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. 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.
Below is the code uploaded to the Axon II MCU http://www.societyofrobots.com/axon2/
#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
#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
/*
* 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
EDIT: I made some changes to the Axon code. I’ll let you know if it works