USE UPS NEXT DAY AIR FOR ANY USA ORDER BEFORE 11AM ET *TODAY* 12/19/2014 TO GET YOUR PACKAGE IN TIME FOR XMAS - PLEASE SEE OUR SHIPPING DEADLINE NOTICE FOR MORE DETAILS!

Trouble With MCU Reading Xbee bytes
Moderators: adafruit_support_bill, adafruit

Trouble With MCU Reading Xbee bytes

by Mastermime on Sun Feb 03, 2013 4:58 pm

Hello everyone,

I am in need of urgent help controlling my motors (Sabertooth 2x25) from a PS2 controller over Xbee wireless. There is probably a small mistake in my code in receiving the serial data from my Xbees. I just don't know what it is. I know the Xbees are properly communicating (tested with X-CTU). When I turn the robot on the motors propel forward until it gets to full speed and then it slowly stops and propel backwards until it gets to full speed. It also does the same thing with my servo. So I believe my MCU is having trouble reading Xbee bytes. It must have to do with my temp byte section.

Below is the code uploaded to the Axon II MCU http://www.societyofrobots.com/axon2/
Code: Select all | TOGGLE FULL SIZE
#include "hardware.h"

#define XBee_Controlled
//#define USB_Controlled

#define TRIANGLE 256
#define START 257
#define L2 258
#define R2 259

#define MOTOR_L_MIN 1
#define MOTOR_L_STOP 64
#define MOTOR_L_MAX 127
#define MOTOR_R_MIN 128
#define MOTOR_R_STOP 192
#define MOTOR_R_MAX 255
#define SIGNAL_MOTORS_OFF 0

int motorLCurVal;
int motorRCurVal;
int motorLTarVal;
int motorRTarVal;
int motorVal;



// Initialise the hardware
void appInitHardware(void) {
   initHardware();
}

// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
   motorLCurVal = motorLTarVal = MOTOR_L_STOP;
   motorRCurVal = motorRTarVal = MOTOR_R_STOP;

   //uartInit(); // initialize the UART (serial port)
   uartInit(Xbee, 9600);         //UART0
   uartInit(uart1, 9600);   //USB
   uartInit(Sabertooth_uart, 9600);   //USB


   
   #ifdef XBee_Controlled
      rprintfInit(XbeeSendByte);
   #endif

   return 0;
}

// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

   int tempbyte = NULL;

   #ifdef XBee_Controlled
      tempbyte=XbeeGetByte;
   #endif

   if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
   {
      setMotorSpeed(tempbyte);
   }
   else if (tempbyte == START)
   {
         pin_high(Remoteswitch_relay);

   }
   else
   {
      motorStopMotor();
   }

   delay_ms(100);

   /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
   TICK_COUNT ms = loopStart / 1000;      // Get current time in ms
   int16_t now = ms % (TICK_COUNT)10000;    // 10 sec for a full swing
   if(now >= (int16_t)5000){            // Goes from 0ms...5000ms
      now = (int16_t)10000 - now;         // then 5000ms...0ms
   }

   return 0;
}

//motorXCurVal is the current speed.
//motorXTarVal is where we want the speed to be.
//motorVal, in this case, is running on a scale of 1 to 10. Thus, 1 is 10% of maximum //speed, 2 is 20%, and so on. NOT USED

void motorStopMotor()
{
   do {
      if (motorLCurVal<MOTOR_L_STOP)
      {
         motorLCurVal=motorLCurVal+1;
      }
      else if (motorLCurVal>MOTOR_L_STOP)
      {
         motorLCurVal=motorLCurVal-1;
      }

      Sabertooth_uartSendByte(motorLCurVal);
      delay_ms(50);

      if (motorRCurVal<MOTOR_R_STOP)
      {
         motorRCurVal=motorRCurVal+1;
      }
      else if (motorRCurVal>MOTOR_R_STOP)
      {
         motorRCurVal=motorRCurVal-1;
      }

      Sabertooth_uartSendByte(motorRCurVal);
      delay_ms(50);

   } while (motorLCurVal!=MOTOR_L_STOP && motorRCurVal!=MOTOR_R_STOP);
}

void setMotorSpeed(int byte)
{
   if (byte >= MOTOR_L_MIN && byte <= MOTOR_L_MAX)
   {
      motorLTarVal = byte;
      do {
         if (motorLCurVal < motorLTarVal)
         {
            Sabertooth_uartSendByte(++motorLCurVal);
         }
         else
         {
            Sabertooth_uartSendByte(--motorLCurVal);
         }
         delay_ms(100);

      } while (motorLCurVal != motorLTarVal);
   }
   else if (byte >= MOTOR_R_MIN && byte <= MOTOR_R_MAX)
   {
      motorRTarVal = byte;
      do {
         if (motorRCurVal < motorRTarVal)
         {
            Sabertooth_uartSendByte(++motorRCurVal);
         }
         else
         {
            Sabertooth_uartSendByte(--motorRCurVal);
         }
         delay_ms(100);

      } while (motorRCurVal != motorRTarVal);
   }
}

/* This is weird and seems unecessarily complicated. Basically the same thing above I think. Not sure why it stops
the motor before moving it as well, seems logically incorrect. Should move from CurSpeed -> TarSpeed, not
CurSpeed -> Stop -> TarSpeed. Keeping this around incase I am wrong.

void motorMoveMotor(int motorL, int motorR) // motorL and motorR are -1 for reverse, 0 for stop, 1 for forward

   motorStopMotor();
   delay_ms(100);
   motorInt=(63*motorVal)/100;
   motorLTarVal=64+motorInt;

   if (motorL<0)
   {
      motorLTarVal=64-motorInt;
   }

   motorRTarVal=192+motorInt;

   if (motorR<0)
   {
      motorRTarVal=192-motorInt;
   }

   do {
      if (motorLCurVal<motorLTarVal)
      {
         motorLCurVal=motorLCurVal+1;
      }
      else if (motorLCurVal>motorLTarVal)
      {
         motorLCurVal=motorLCurVal-1;
      }

      motors_uartSendByte(motorLCurVal);
      delay_ms(50);

      if (motorRCurVal<motorRTarVal)
      {
         motorRCurVal=motorRCurVal+1;
      }
      else if (motorRCurVal>motorRTarVal)
      {
         motorRCurVal=motorRCurVal-1;
      }

      motors_uartSendByte(motorRCurVal);
      delay_ms(50);
      
   } while (motorLCurVal!=motorLTarVal && motorRCurVal!=motorRTarVal);

}
*/



Below is the code generated from Webbotlib Project Designer

Code: Select all | TOGGLE FULL SIZE
#include "hardware.h"

// Initialise the hardware
void appInitHardware(void) {
   initHardware();
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
   return 0;
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

   // -------- Start Switch/Button-------
   // Switch/Button - see switch.h
   
   // To test if it is pressed then
   if(SWITCH_pressed(&button)){
      // pressed
   }
   
   // To test if it is released then
   if(SWITCH_released(&button)){
      // released
   }
   // -------- End   Switch/Button-------

   // -------- Start Marquee-------
   // Marquee - see 'segled.h'
   // Before using the Marquee you need to redirect rprintf to write to it
   // This can be done using
   Writer old = rprintfInit(marqueeGetWriter(&marquee));
   
   // All rprintf output will then be sent to the marquee but will not
   // display until an end-of-line eg "\n" has been sent. Example:-
   // rprintf("Hello World\n");
   
   // If the endDelay is non-zero then the marquee will scroll
   // forever or until you call: marqueeStop(&marquee);
   
   // If the endDelay is zero then the marquee will stop once
   // the entire line has been shown ('one-shot' mode)
   
   // In 'one-shot' mode then you may want to make sure that
   // a previous line has finished before you display a second line.
   // This can be done as follows:-
   marqueeSetEndDelay(&marquee,0); // Make sure we are in one-shot mode
   if(marqueeIsActive(&marquee)==FALSE){
        if(loopCount==1){
           rprintf("ABCDEFGHIJKLMNOPQRSTUVWXYZ\n");
        }else{
         rprintf("Loop=%u\n",(unsigned)loopCount); // Put the loop count
        }
   }
   
   // Restore rprintf back to its previous location
   rprintfInit(old);
   // -------- End   Marquee-------

   // -------- Start Analogue Input-------
   // Read the Analogue Input and store results
   uint16_t Temp_sensor1val = a2dConvert10bit(Temp_sensor1);
   // Dump out the value
   rprintf("Temp_sensor1: %d\n",Temp_sensor1val);
   // -------- End   Analogue Input-------

   // -------- Start Analogue Input-------
   // Read the Analogue Input and store results
   uint16_t Temp_sensor2val = a2dConvert10bit(Temp_sensor2);
   // Dump out the value
   rprintf("Temp_sensor2: %d\n",Temp_sensor2val);
   // -------- End   Analogue Input-------

   // -------- Start Analogue Input-------
   // Read the Analogue Input and store results
   uint16_t Bat_monitorval = a2dConvert10bit(Bat_monitor);
   // Dump out the value
   rprintf("Bat_monitor: %d\n",Bat_monitorval);
   // -------- End   Analogue Input-------

   // -------- Start LED-------
   // The LED can be manipulated using the calls in led.h
   
   // To turn the LED on:-
   LED_on(&led_ext1);
   
   // To turn the LED off:-
   LED_off(&led_ext1);
   // -------- End   LED-------

   // -------- Start LED-------
   // The LED can be manipulated using the calls in led.h
   
   // To turn the LED on:-
   LED_on(&led_ext2);
   
   // To turn the LED off:-
   LED_off(&led_ext2);
   // -------- End   LED-------

   // -------- Start LED-------
   // The LED can be manipulated using the calls in led.h
   
   // To turn the LED on:-
   LED_on(&leds_int);
   
   // To turn the LED off:-
   LED_off(&leds_int);
   // -------- End   LED-------

   // -------- Start Digital Output-------
   // Set the pin high
   pin_high(Remoteswitch_relay);
   
   // Set the pin low
   pin_low(Remoteswitch_relay);
   
   // Toggle the pin ie high->low, or low->high
   pin_toggle(Remoteswitch_relay);
   
   // Output a high pulse of 1000us
   pin_pulseOut(Remoteswitch_relay,1000,TRUE);
   // -------- End   Digital Output-------

   // -------- Start Digital Output-------
   // Set the pin high
   pin_high(Xbee5v);
   
   // Set the pin low
   pin_low(Xbee5v);
   
   // Toggle the pin ie high->low, or low->high
   pin_toggle(Xbee5v);
   
   // Output a high pulse of 1000us
   pin_pulseOut(Xbee5v,1000,TRUE);
   // -------- End   Digital Output-------

   // -------- Start Actuators -------
   // To control your.motors/servos then see actuators.h in the manual
   // To retrieve the required speed of motor_1 use:
   // DRIVE_SPEED speed=act_getSpeed(motor_1);
   // To set the required speed of motor_1 use:
   // act_setSpeed(motor_1,speed);
   // This example will move the motors back and forth using the loopStart time:
   TICK_COUNT ms = loopStart / 1000;      // Get current time in ms
   int16_t now = ms % (TICK_COUNT)10000;    // 10 sec for a full swing
   if(now >= (int16_t)5000){            // Goes from 0ms...5000ms
      now = (int16_t)10000 - now;         // then 5000ms...0ms
   }
   // Map it into DRIVE_SPEED range
   DRIVE_SPEED speed = interpolate(now, 0, 5000, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
   // Set speed for all motors/servos
   act_setSpeed(&motor_1,speed);
   act_setSpeed(&motor_2,speed);
   act_setSpeed(&servo,speed);
   // -------- End   Actuators -------

   return 0;
}


Below is the code I uploaded to the Arduino


Code: Select all | TOGGLE FULL SIZE
/*
 * NOTES:
 * 0-127 Motor Left (1-63: forward, 64: stop, 65-127: reverse)
 * 128-255 Motor Right (128-191: forward, 192: stop, 193-255: reverse)
 */


#include <GPSXClass.h>
#include <SoftwareSerial.h>
#include <LiquidCrystal.h>
//#include <XBee.h>
#include <Servo.h>
#include <Sabertooth.h>
// LCD SCREEN
#define LCD_COLS 20
#define LCD_ROWS 4
#define BACKLIGHT_ON 'HIGH'
#define BACKLIGHT_OFF 'LOW'
// XBEE
#define DATA_LED 10
#define STATUS_LED 11
#define ERROR_LED 12
#define REMOTE_XBEE_ADDR 0x1874 //TODO
#define RESPONSE_WAIT 5000
#define DEBUG true
// PS2 SEND
#define TRIANGLE 256
#define START 0
#define L2 257
#define R2 258
#define JOYSTICK_LOW 90
#define JOYSTICK_HIGH 160

#define DELAY 100
#define SERIAL_BPS 9600

// GLOBAL VARIABLES
LiquidCrystal lcd(12, 11, 5, 4, 3, 2); //TODO


/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
   Initialize
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void setup()
{
   initPSX(); //Get PS2 controller ready
   initLCD(); //Get LCD ready
   initXBee(); //Get XBee Wireless Transmitter ready
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
   Main
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void loop()
{
    //PS2 Controller Input
    readPS2Input();

   delay(DELAY);
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
   Helpers
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

void initPSX()
{
   PSX.mode(PSX_PAD1, MODE_ANALOG, MODE_LOCK);
   PSX.motorEnable(PSX_PAD1, MOTOR1_DISABLE, MOTOR2_DISABLE);
   // Poll current state once.
   PSX.updateState(PSX_PAD1);
}

void initLCD()
{
   lcd.begin(LCD_COLS, LCD_ROWS);
   lcd.clear();
   lcd.setCursor(0,0);
   //lcd.print("Robot Started");
}

void initXBee()
{
   Serial.begin(9600);
}

void readPS2Input()
{
     Serial.begin(9600);
 
   PSX.updateState(PSX_PAD1);

   if(PRESSED_TRIANGLE(PSX_PAD1))
   {
      if (DEBUG) Serial.println("Triangle pressed");
      else sendData(TRIANGLE);
   }
   if(PRESSED_START(PSX_PAD1))
   {
      if (DEBUG) Serial.println("Start pressed");
      else sendData(START);
   }
   if(IS_DOWN_L2(PSX_PAD1))
   {
      if (DEBUG) Serial.println("L2 down");
      else sendData(L2);
   }
   if(IS_DOWN_R2(PSX_PAD1))
   {
      if (DEBUG) Serial.println("R2 down");
      else sendData(R2);
   }

   int leftStick = ANALOG_LEFT_Y(PSX_PAD1);
   int rightStick = ANALOG_RIGHT_Y(PSX_PAD1);
   if(leftStick < JOYSTICK_LOW)
   {
      if (DEBUG)
      {
         Serial.print("LEFT TRIGGER FORWARD: ");
         Serial.println(ANALOG_LEFT_Y(PSX_PAD1), DEC);
      }
      else sendData(map(leftStick, 0, JOYSTICK_LOW, 127, 65));
   }
   if(leftStick > JOYSTICK_HIGH)
   {
      if (DEBUG)
      {
         Serial.print("LEFT TRIGGER BACKWARDS: ");
         Serial.println(ANALOG_LEFT_Y(PSX_PAD1), DEC);
      }
      else sendData(map(leftStick, JOYSTICK_HIGH, 255, 63, 1));
   }
   if(rightStick < JOYSTICK_LOW)
   {
      if (DEBUG)
      {
         Serial.print("RIGHT TRIGGER FORWARDS: ");
         Serial.println(ANALOG_RIGHT_Y(PSX_PAD1), DEC);
      }
      else sendData(map(rightStick, 0, JOYSTICK_LOW, 255, 193));
   }
   if(rightStick > JOYSTICK_HIGH)
   {
      if (DEBUG)
      {
         Serial.print("RIGHT TRIGGER BACKWARDS: ");
         Serial.println(ANALOG_RIGHT_Y(PSX_PAD1), DEC);
      }
      else sendData(map(rightStick, JOYSTICK_HIGH, 255, 191, 128));
   }
}

void sendData(int sendVal)
{
   Serial.print(sendVal, DEC);
delay(10);
}



Thanks for reading this
Mastermime
 
Posts: 49
Joined: Mon Oct 22, 2012 7:46 pm