{Feedback Servo Demo} CON _clkmode = xtal1 + pll16x 'Standard clock mode _xinfreq = 5_000_000 '* crystal frequency = 80 MHz CP0 = 17, FP0 = 8, Servo0 = 0 'servo zero pins, name CP1 = 16, FP1 = 9, Servo1 = 1 'servo one: Make these zeroes for single servo case CP2 = 15, FP2 = 10, Servo2 = 2 'make these zeroes for servos CP3 = 14, FP3 = 11, Servo3 = 3 'that do not exist Encoder = 12 '32-click encoder input #0, CW, CCW 'directions for velocity VAR byte Cog word commanded 'commanded angle byte angle 'which of four cases for full scale changes long random 'chooses random angle word rcount 'counts random positionings word Cmnded[4] 'commanded angle byte currentservo 'used in random positioning byte TheServo byte LastServo word Servo2Theta 'used in final demo long work1 long timer OBJ pst : "parallax serial terminal" FBS : "FourPlyVersion3" PUB main pst.Start (115_200) 'start up ser terminal pst.str(String("hello, world ")) 'runs in cog 1 pst.NewLine waitcnt(clkfreq/10+cnt) cog := FBS.start(CP0,FP0, CP1,FP1, CP2,FP2, CP3,FP3) 'specify pin numbers pst.str(string("Started PASM cog: ")) '0,0 pin numbers says no servo pst.dec(cog) pst.newline waitcnt(clkfreq/2+cnt) 'allow time for it to start pst.str(string("Setting the DeadZone value(s) for my specific Servo(s)")) pst.newline pst.str(string("Your values will almost certainly vary")) pst.newline pst.str(string("The default is 31")) pst.newline FBS.setDeltaMin(servo0, 24) if (LastServo > 0) FBS.setDeltaMin(servo1, 29) if (LastServo > 1) FBS.setDeltaMin(servo2, 25) if (LastServo > 2) FBS.setDeltaMin(servo3, 33) pst.newline pst.newline pst.str(string("Test Constant velocity mode using Servo2 only")) pst.newline pst.str(string("Displays the actual pulse width sent to the servo")) pst.newline pst.str(string("As you increase the load,")) pst.newline pst.str(string("Observe that the pulse width deviates furthur from 1500")) pst.newline pst.str(string("Also displays the really-truly RPM as sensed by a Bot encoder.")) pst.newline pst.str(string("Hit any key to begin or stop")) commanded := 60 ctra := (%01010 << 26) | Encoder 'pos edge detect frqa := 1 'count by one timer := cnt repeat until pst.rxcount <> 0 pst.char(pst.charin) FBS.SetVelocity(servo2, commanded, CW) repeat until pst.rxCount <> 0 pst.newline pst.str(string("Commanded RPM: ")) pst.dec(commanded) pst.str(string(" Pulse Width: ")) pst.dec(FBS.GetPulseWidth(servo2)) pst.str(string(" Reported RPM: ")) pst.dec(FBS.GetVelocity(servo2)) pst.str(string(" Actual RPM:")) phsa := 0 waitcnt(clkfreq + cnt) 'one second pst.dec((phsa * 60) / 32) FBS.Width(servo2, 1500) 'stop the servo pst.char(pst.charin)