Trouble With MCU Reading Xbee bytes

General project help for Adafruit customers

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
Locked
User avatar
mastermime
 
Posts: 50
Joined: Mon Oct 22, 2012 7:46 pm

Trouble With MCU Reading Xbee bytes

Post by mastermime »

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

#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

#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

/*
 * 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

Locked
Please be positive and constructive with your questions and comments.

Return to “General Project help”