Learning PASM2 and cordic!
I've been learning PASM to convert some math intensive routines over to PASM for improved speeds. In the process I've learned lots of new things and spent a lot of time digging into whatever documentation and examples I could get my hands on.
I wanted to share some of the things I learned that might help someone else who is new to P2 PASM.
First time creating new PASM code was difficult and figuring out each next step was pretty tedious and I wasn't proceeding very well. So I used SPIN to code the math routine first. This code was worked on until I was getting the expected output. I wrote the SPIN as a series of steps so I could break down the sequence and this was used as a guideline. Now I was writing out only a line of 2 of SPIN at a time, much easier to keep track of everything.
I coded the PASM using the same routine as the SPIN routine (might have been easier to create separate routines but this worked as I bounced back and forth between SPIN and PASM. Debug statements are after most SPIN code lines to match the SPIN values with the equivalent PASM values. If they didn't match or I got an error, then I had something new to learn about PASM!
pub GetXYLegPosition(femurA, tibiaA, coxaA, body):x1, y1 | f1, t1, c1, c2, femurL, tibiaL, coxaL, scale1, degree1, tmp1, step1, step2, step3, step4, step5, step6, step7, step8 'calculate distances using angles in Forward Kinematics 'input 3 leg angles and body from the floor distance 'return leg Cartesian location in x,y coordinates 'uses pr0-pr2 as additional temporary variable registers femurL := femurLength tibiaL := tibiaLength coxaL := coxaLength org mov f1, ##1800 sub f1, femurA mov t1, tibiaA add t1, femurA mov c1, coxaA cmp c1, ##900 wcz if_a sub c1, ##1800 abs c1, c1 debug (sdec(f1), sdec(t1), sdec(c1)) mov tmp1, femurL mul tmp1, tmp1 'femurL squared mov pr0, tibiaL mul pr0, pr0 'tibia squared add tmp1, pr0 'add values and store in tmp1 mov pr1, tmp1 'copy tmp1 over to pr1 debug (sdec(tmp1), sdec(pr1)) mov pr0, femurL mul pr0, #2 'mult femurL by 2 mul pr0, tibiaL 'mult tibiaL by prev results, store in pr0 debug (sdec(pr0)) mov scale1, ##10000 'cosine start mov degree1, ##$80000000 setq #0 qdiv degree1, ##1800 getqx degree1 ' debug (sdec(degree1)) qmul degree1, t1 getqx tmp1 ' debug (sdec(tmp1)) qrotate scale1, tmp1 ' debug (sdec(scale1), sdec(tmp1)) getqx tmp1 'cosine value in tmp1 debug (sdec(tmp1)) qmul tmp1, pr0 'mult cosine by pr0 getqx tmp1 'store in tmp1 qdiv tmp1, ##10000 'div by 10000 to get in right range getqx tmp1 'store in tmp1 ' debug (sdec(tmp1)) sub pr1, tmp1 'sub pr0 from pr1, result in pr1 ' debug (sdec(pr1), sdec(pr0)) qsqrt pr1, #0 'take square root getqx pr2 'store square root in pr2 ' debug (sdec(pr2)) mul pr2, pr2 'square pr2 mul body, body 'square body height sub pr2, body 'subtract values, store in pr2 qsqrt pr2, #0 getqx tmp1 'store value in tmp1 ' debug (sdec(tmp1)) add tmp1, coxaL 'get total length, value of side from tip to femur pivot ' debug (sdec(tmp1)) mov degree1, ##$80000000 'convert coxaA to value readable by pasm cordic setq #0 qdiv degree1, ##1800 getqx degree1 qmul degree1, c1 getqx degree1 'degree1 is c1 coxaA ready for cordic ' debug (sdec(tmp1), sdec(degree1)) qrotate tmp1, degree1 getqx x1 getqy y1 ' debug (sdec(x1), sdec(y1)) end f1 := 1800 - 701 'femurAngle[n] t1 := 4 + 701 'tibiaAngle[n] + femurAngle[n] c2 := 1000 c1 := c2 if c1 > 900 c1 := 1800 - c1 'convert angle to CCW rotation for math debug("Leg XY angle positions: Leg ", sdec(f1), sdec(t1), sdec(c1)) 'test faster calculations step1 := t1 'get angle debug(sdec(step1)) step2 := (femurLength*femurLength) + (tibiaLength*tibiaLength) debug(sdec(step2)) step3 := 2*femurLength*tibiaLength debug(sdec(step3)) step4 := abs(qcos(10000,step1,3600)) debug(sdec(step4)) step5 := (step3 * step4)/10000 debug(sdec(step5)) step6 := step2 - step5 debug(sdec(step6)) step7 := sqrt(step6) debug(sdec(step7)) step8 := sqrt((step7*step7) - (bodyz*bodyz)) debug(sdec(step8)) step1 := step8 + coxaLength debug(sdec(step1)) step2 := abs(c1 * ($80000000 / 180)) debug("c1 conversion: ", sdec_(step2)) x1, y1 := polxy(step1, step2) if c2 < 900 step3 := step3 * -1 debug("Results: ", "Hypot: ", sdec_(step1), " angle: ", sdec_(step2), " x: ", sdec_(x1), " y: ", sdec_(y1))
Found a limitation of 16 registers (for variables) in PASM for a routine. This includes the variables passed to the routine, return values, and local variables. From the SPIN docs there are an additional 8 registers labeled pr0 through pr7 that can also be used for any purpose and don't count toward that 16 register limit.
Qrotate was interesting to figure out, there were several discussions about using it but little code that I could use. Getting the input angle into the right format was the hardest as I wanted to input 1/10 degree increments so scaling the value was required. Another thing that tripped me up for a while was multiplication sequences that exceeded 32 bits, a problem until I finally realized what was happening and scaled the values down enough to stay inside the 32 bit limit.
I really liked using qrotate to get sine and cosine by making the hypotenuse value = 1 or a scaled value!
The ability to use direct integer values using # in front of the value was great but when I tried that with a value >512 I got an error. Took a while but digging in the docs I finally found that ## allows number up to 32 bits.
I couldn't find much on qsqrt example wise, still don't know what the second input value is supposed to be but putting qsqrt value1, #0 works.
I looked but couldn't find a way to use the SPIN declared constants in PASM without using local variables in the routine.
This method worked well for me to keep me focused on using small steps and on the specific task I needed. I'm sure this is a long way around for the experienced PASM coders out there but it worked well in my case.
Next item is to use a variety of angle inputs to make sure it doesn't break and work to see if there is any way to simplify and speed up the routine. I know cordic routines can be stacked but the way I am using them the results of one cordic is needed for the next cordic so not sure that option is available to me here.
Suggestions for improvements are welcome and definitely let me know if I misstated something or have the wrong interpretation of what I saw.