BT Servo Code Help Needed

NWCCTV · 2015-02-25 21:46

In the below code the xpos == 0 and ypos == -99 works but the xpos == 0 and ypos == 0 will only work if I comment out the xpos == 0 and xpos == -99 portion. Any suggestions?

pub get_joystick

'' Process joystick command from JBTC
'' -- JBTC sends joystick packet as <STX> <X100> <X10> <X1> <Y100> <Y10> <Y1> <ETX>
''    * x and y digits are ascii chars
  
  xpos := get_dec3(@cmd[1])
  ypos := get_dec3(@cmd[4]) 

  if xpos == 0 and ypos == 0
    servo.Set(0, 200)                                    ' P0 servo full speed clockwise
    servo.Set(1, -200)                                   ' P1 servo full speed counterclockwise
  else
    servo.Set(0, 0)                                      ' P0 servo stop
    servo.Set(1, 0)                                      ' P1 servo stop 

  if xpos == 0 and ypos == -99
    servo.Set(0, -200)                                   ' P0 servo full speed counterclockwise
    servo.Set(1, 200)                                    ' P1 servo full speed clockwise
  else
    servo.Set(0, 0)                                      ' P0 servo stop
    servo.Set(1, 0)                                      ' P1 servo stop

twm47099 · 2015-02-26 06:33

Andy,
I'm not a Spin expert, but I think that this is what is happening. After processing

if xpos == 0 and ypos == 0

as true and starting the servos, the code then goes to:

if xpos == 0 and ypos == -99

which is false and then does the else part of that if-else segment and stops. The code is fast enough that the servo doesn't have a chance to move before it stops.

One way of making it work is to put one of the if statements into the else of the other. In pseudo code:

if...
else if...
else

However, if I wanted to use the joystick values to control the speed I would calculate the servo values directly from the joystick value. This is how I did it in C using the SimpleIDE activitybot libraries. Something similar could be done in Spin using the required servo values scalled to the speed you want. There may even be an OBEX continuous servo object that could be used??

I do a similar thing for using a standard servo if I am controlling one with JSBTC. Again in C I use the Simple Libraries which takes a command in degrees, but I've done it in forth calculating the required servo value.

This is how I did it in C:

      {
  //  x2 and y2 are the values of x & y from my get_joy function (-100 to +100)
 
       yspd = (abs(y2) / 15) * 15;         // scale the fwd & rev motion  (if x = 0 go straight)
       if(y2 <0) yspd = yspd / -2;         //  go slower in reverse  
       xspd = (abs(x2) / 15) * 4;          //  scale the turning
       if(x2<0) xspd = xspd * -1;
  
       drive_ramp(yspd+xspd, yspd-xspd);         // drive, turning differential added to yspd  
  // Note the ABDrive Library does not require that one servo value be + and the other - for the wheels to spin in the same direction.
     }

Note that in the code above y2 is divided by 15 and then multiplied by 15 (x2 is divided by 15 and multiplied by 4). That is just because I wanted any value between -15 and +15 to be neutral (no movement) so I wouldn't have to exactly center the joystick to stop the bot.

Hope this helps
Tom

BT Servo Code Help Needed

Comments