I have built a three wheeled robot with omni wheels, and all of the wheels are 60° from each other, and I'm using continuous rotation servos for my motors. I'm trying to control it with a radio controlled system, but I can't mix my channels like I need in my radio, so I wrote some code to read the channel 1 signal (up and down on the right joystick, forward and reverse), channel 2 signal (right and left on the right joystick, rotate clockwise and counterclockwise), and channel 4 signal (left and right on the left joystick, slide sideways left and right) via the pulsin command. I then mixed those signals together and outputted the data via a servopos command. Using a sertxd command I can read my variable values and they are exactly what I expect them to be, but when I run the code (with sertxd commented out) the servos are very, very jittery, and when I read their signals with an oscilloscope it is a very erratic signal.
I use my input signals from the receiver as my trigger on channel 1 on my oscilloscope, and my servopos signal (on channel 2) is maybe 4 times as long as it should be, then an appropriate space (about 20ms) and a pulse that looks correct, then it takes 3 or more transmitter input pulses before it does the long-signal correct-signal thing again, so it's not accurately pulsing a servo signal every 20ms, nor are all of the pulse lengths correct. I would guess I'm not going to get all of those calculations done in between the 20ms pulses, but that's probably OK for me, but I need my servopos signal to be operating correctly while it's doing the math for the next change in pulse length, but it's not.
Is there a trick to using pulsin and servopos at the same time? This is on a 14M2.
I use my input signals from the receiver as my trigger on channel 1 on my oscilloscope, and my servopos signal (on channel 2) is maybe 4 times as long as it should be, then an appropriate space (about 20ms) and a pulse that looks correct, then it takes 3 or more transmitter input pulses before it does the long-signal correct-signal thing again, so it's not accurately pulsing a servo signal every 20ms, nor are all of the pulse lengths correct. I would guess I'm not going to get all of those calculations done in between the 20ms pulses, but that's probably OK for me, but I need my servopos signal to be operating correctly while it's doing the math for the next change in pulse length, but it's not.
Is there a trick to using pulsin and servopos at the same time? This is on a 14M2.
Code:
symbol forwards = b0 ;to be used in pulsin
symbol rotate = b1 ;to be used in pulsin
symbol slide = b2 ;to be used in pulsin
symbol flforward = b3 ;front left forward from forwards variable
symbol frforward = b4 ;front right forward from forwards variable
symbol bforward = 150 ;will always be 150
symbol flrotate = b5 ;front left rotate from rotate variable
symbol frrotate = b6 ;front right rotate from rotate variable
symbol brotate = b7 ;back rotate from rotate variable
symbol flslide = b8 ;front left slide from slide variable
symbol frslide = b9 ;front right slide from slide variable
symbol bslide = b10 ;back slide from slide variable
symbol fl = w6 ;front left
symbol fr = w7 ;front right
symbol ba = w8 ;back
symbol fldif = b18 ;difference between fl and 150
symbol frdif = b19 ;difference between fr and 150
symbol bdif = b20 ;difference between ba and 150
symbol tempone = w11 ;for holding math sub-calculations
symbol temptwo = w12 ;for holding math sub-calculations
Servosetup:
;disconnect
;low b.2
servo b.3, 150 ;front left servo
servo b.4, 150 ;front right servo
servo b.5, 150 ;back servo
pause 5000
Main:
;get readings
pulsin c.2, 1, forwards ;measure servo pulse lengtth (do I need a word variable?)
pulsin c.1, 1, rotate ;measure servo pulse length for rotate
pulsin c.0, 1, slide ;measure servo pulse length for slide
;convert forwards signal into flforward and frforward
let flforward = forwards
let frforward = 300-forwards
;convert rotate signal into flrotate, frrotate, and brotate
let flrotate = rotate
let frrotate = rotate
let brotate = rotate
;convert slide signal into flslide, frslide, and bslide
let flslide = slide+150/2
let frslide = slide+150/2
let bslide = 300-slide
;add front lefts, front rights, and backs
let fl = flforward + flrotate + flslide - 300
let fr = frforward + frrotate + frslide - 300
let ba = bforward + brotate + bslide - 300
;find each signal's difference from 150, possible input range is 0-300
if fl > 150 then
let fldif = fl-150
else
let fldif = 150-fl
endif
if fr > 150 then
let frdif = fr-150
else
let frdif = 150-fr
endif
if ba > 150 then
let bdif = ba-150
else
let bdif = 150-ba
endif
;find the greatest distance signal from 150, its slope, and y intercept, then scale signals
if fldif >= frdif and fldif >= bdif and fldif > 50 then ;when fl is biggest and greater than 50
;calculate new fr
let tempone = 50 * fr
let temptwo = 7500 / fldif
let fr = tempone / fldif + 150 - temptwo
;calculate new ba
let tempone = 50 * ba
let temptwo = 7500 / fldif
let ba = tempone / fldif + 150 - temptwo
;calculate new fl
if fl > 200 then
let fl = 200
endif
if fl < 100 then
let fl = 100
endif
endif
if frdif >= fldif and frdif >= bdif and frdif > 50 then ;when fr is biggest and greater than 50
;calculate new fl
let tempone = 50 * fl
let temptwo = 7500 / frdif
let fl = tempone / frdif + 150 - temptwo
;calculate new ba
let tempone = 50 * ba
let temptwo = 7500 / frdif
let ba = tempone / frdif + 150 - temptwo
;calculate new fr
if fr > 200 then
let fr = 200
endif
if fr < 100 then
let fr = 100
endif
endif
if bdif >= fldif and bdif >= frdif and bdif > 50 then ;when bdif is biggest and greater than 50
;calculate new fl
let tempone = 50 * fl
let temptwo = 7500 / bdif
let fl = tempone / bdif + 150 - temptwo
;calculate new fr
let tempone = 50 * fr
let temptwo = 7500 / bdif
let fr = tempone / bdif + 150 - temptwo
;calculate new ba
if ba > 200 then
let ba = 200
endif
if ba < 100 then
let ba = 100
endif
endif
;sertxd("fl ",#fl," fr ",#fr," ba ",#ba,13,10)
servopos b.3, fl ;output to front left
servopos b.4, fr ;output to front right
servopos b.5, ba ;output to back
goto main