Controlling motors [Urgent]

[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.  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

Code: [Select]

#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) {
//uartInit(); // initialize the UART (serial port)
uartInit(Xbee, 9600); //UART0
uartInit(uart1, 9600); //USB
uartInit(Sabertooth_uart, 9600); //USB

SabertoothInit(&driver);   //Initialize sabertooth

}


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

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

int tempbyte = NULL;

tempbyte=XbeeGetByte;  // Xbee byte

if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
{
setMotorSpeed(tempbyte);
}
else if (tempbyte == START)
{
pin_toggle(Remoteswitch_relay);   // Turn relay on or off
}
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;
}
//code by Tzankoff
//code for gradual stopping and starting
//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]

#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]

/*
 * 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 just changed some things to the Axon code.  I'll let you know if it works

[Mastermime]

Uhh still no luck.  Please help

[waltr]

Break down the hardware and software into smaller functional blocks then test and debug each block.
1- Is there an issue between the Axon, Arduino and Sabertooth?
    Which processor is actually controlling the Sabertooth?

2- Is there a communication issue on the serial link?
  Get the serial data link working between the processors without the XBees by hard wiring the serial line between the two processors.
 
3- Is there an XBee setup and communication issue?
   Get the XBees working with a simple to test code. One way is just send data one way then send the data back (remote Echo).

4- Is the PS2 controller interface correct? Have you tested and confirmed this?
 
5- Is the command protocol is the serial communication chain working correctly?
  Hard code values at the receiving end to test that code is correctly using the commands.
  Echo the command back to ensure they are getting received intact.

6- Also use a PC (or MAC or Linx) serial data port to test the serial to each processor.
  Send commands to mimic the data from the PC to each processor.
  Send the PS2 data to the PC to test if the data is being send properly.
  Show us a block diagram of the system and what connects to what. So far we can only guess.

Last, your project is Not Urgent to us.

[Mastermime]

Thanks a lot for the response.  I really appreciate it

Is there an issue between the Axon, Arduino and Sabertooth?
    Which processor is actually controlling the Sabertooth?

The Axon II is controlling the Sabertooth 2x25

2- Is there a communication issue on the serial link?
  Get the serial data link working between the processors without the XBees by hard wiring the serial line between the two processors.

I have tested and commands are properly being sent from one Xbee to another.  So you're saying I should just put a wire between tx and rx and vice versa on the Arduino and Axon II, right?  I don't think that's the problem, but it may be.  I think there is an error in my code.

3- Is there an XBee setup and communication issue?
   Get the XBees working with a simple to test code. One way is just send data one way then send the data back (remote Echo).

See above.

6- Also use a PC (or MAC or Linx) serial data port to test the serial to each processor.
  Send commands to mimic the data from the PC to each processor.
  Send the PS2 data to the PC to test if the data is being send properly.
  Show us a block diagram of the system and what connects to what. So far we can only guess.

Yep, that's exactly how I've been debugging.  Through XCTU and the Arduino serial monitor


This is how data is being sent   

Ps2 controller > Arduino Uno R3 (processes commands from Ps2) > Xbee transmitter > Xbee receiver > Axon II > Sabertooth

Hopefully that helps clears things up


I have narrowed down the problem to the reading of the data being sent to the Axon II from the Xbee (Xbee receiver > Axon II).  There has to be something wrong with my Axon code. 

[Mastermime]

I must also add the Marquee is blinking randomly even though I didn't program it to or at least I don't think I did.  Does this mean the Axon could be resetting?  Just a guess. 

Also I have a remote switch I'm having issues with too.  How it works  Xbee Digital Output > to MOSFET (connects grounds) > energize relay coil and now power can flow to the Sabertooth.  When I connect to a 12v signal, everything turns on, but when  I connect it to the digital output, the relay makes a loud buzzing sound.  Any ideas what this could be?  Sorry for all the issues

[Admin]

The fact that your motors and servos are ramping up and then reversing is definitely a software bug. I see a lot of ++ and -- in your code, in while loops. I'd bet that is the source of it. I recommend commenting your code so that you can work through it logically in your head, and so we can follow your code easier too   

I must also add the Marquee is blinking randomly even though I didn't program it to or at least I don't think I did.  Does this mean the Axon could be resetting?  Just a guess.

Possibly. I'd check your battery voltages. The schematic in your other post only showed one battery. Try using a separate battery for your microcontroller and see if this still happens.

Also I have a remote switch I'm having issues with too.  How it works  Xbee Digital Output > to MOSFET (connects grounds) > energize relay coil and now power can flow to the Sabertooth.  When I connect to a 12v signal, everything turns on, but when  I connect it to the digital output, the relay makes a loud buzzing sound.  Any ideas what this could be?  Sorry for all the issues

You mean with 12V connected to the relay, it works fine? But when you connect the MOSFET output to the relay it makes the buzzing noise?

Is it possible that the MOSFET output isn't 12V, but lower? Or the MOSFET output is pulsing a square wave for some odd reason? Got a oscope to measure it?

[Mastermime]

Thank you for the response Admin.

I have Good news and bad news. 
Good news- all my wiring is correct.  The Sabertooth turns on as it should because I set the pin high and the marquee is not blinking erratically.  The status led blinks when I turn the corresponding Xbee on.  The motors are not shooting off when the Sabertooth turns on which is good.


Bad news-  No commands are being read by the Axon II

So I think I should first work on turning the relay on off

[Mastermime]

Ok so I know it hasn't something to do with the Axon II.  First thing I did wrong, was I sent 0 as triangle, but my motors still should've turned regardless.  Is there a way to look at the output of a UART (Sabertooth) port?  I can use an FTDI cable, cant I?

[Mastermime]

I've updated some code, but still no luck.

Axon II Code

Code: [Select]


#include "hardware.h"

#define XBee_Controlled
//#define USB_Controlled

#define UART0_TX_BUFFER_SIZE 4
#define UART0_RX_BUFFER_SIZE 4

#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;
}

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

int tempbyte = NULL;

#ifdef XBee_Controlled
//tempbyte=XbeeGetByte;
if(!uartReceiveBufferIsEmpty(UART0)) {
int tempbyte = uartGetByte(UART0);
        }
#endif

if (tempbyte != NULL) {

if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
{
setMotorSpeed(tempbyte);
}
else if (tempbyte==START)
{
pin_low(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);

}
*/

Arduino Code

Code: [Select]

/* 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 false
// PS2 SEND
#define TRIANGLE 256
#define START 257
#define L2 258
#define R2 259
#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);
}
Also, here is my hardware.h file

Code: [Select]

/*
This file has been auto-generated by WebbotLib tools V1.1
** DO NOT MODIFY BY HAND **
*/
#ifndef _HARDWARE_H_
#define _HARDWARE_H_

#if !defined (_LIB_HARDWARE_C_)  && !defined(_LIB_HARDWARE_CPP_)
#define BUILDING_LIBRARY
#define _LIB_HARDWARE_C_
#endif

#ifndef F_CPU
#warning No CPU speed specified - assuming running at 16000000
#define F_CPU 16000000
#endif

#if F_CPU != 16000000
# warning "Board runs at 16000000 but you have defined a different value"
#endif

#if defined (__AVR_ATmega640__)
#else
# error You must set the device to the ATmega640
#endif

#define FLASH_SIZE 65536
#define RAM_SIZE 8192
#define EEPROM_SIZE 4096
#define _NUM_PCINT_PINS 9

// Include library files
#include <libdefs.h>
#include <core.h>
#include <timer.h>
#include <a2d.h>
#include <rprintf.h>
#include <led.h>
#include <stdlib.h>
#include <avr/eeprom.h>
#include <switch.h>
#include <errors.h>
#include <uart.h>
#include <segled.h>
#include <Motors/DimensionEngineering/Sabertooth.h>
#include <iopin.h>
#include <servos.h>
#include "lib/lib_timerdef.h"
#include "lib/lib_iopins.h"
#ifdef __cplusplus
extern "C" {
#endif

// ------------------- uart1 -------------------
extern MAKE_WRITER(uart1SendByte);
extern MAKE_READER( uart1GetByte);

// Create hardware UART uart1
extern HW_UART _uart1;
#define uart1 &_uart1
#define UART1 uart1

// ------------------- Xbee -------------------
extern MAKE_WRITER(XbeeSendByte);
extern MAKE_READER(XbeeGetByte);

// Create hardware UART Xbee
extern HW_UART _Xbee;
#define Xbee &_Xbee
#define UART0 Xbee

// ------------------- Sabertooth_uart -------------------
extern MAKE_WRITER(Sabertooth_uartSendByte);

// Create hardware UART Sabertooth_uart
extern HW_UART _Sabertooth_uart;
#define Sabertooth_uart &_Sabertooth_uart
#define UART2 Sabertooth_uart

// ----------- Define the ADC channels ----------
#define ADC0 ADC_NUMBER_TO_CHANNEL(0)
#define ADC1 ADC_NUMBER_TO_CHANNEL(1)
#define ADC2 ADC_NUMBER_TO_CHANNEL(2)
#define ADC3 ADC_NUMBER_TO_CHANNEL(3)
#define ADC4 ADC_NUMBER_TO_CHANNEL(4)
#define ADC5 ADC_NUMBER_TO_CHANNEL(5)
#define ADC6 ADC_NUMBER_TO_CHANNEL(6)
#define ADC7 ADC_NUMBER_TO_CHANNEL(7)
#define ADC8 ADC_NUMBER_TO_CHANNEL(8)
#define ADC9 ADC_NUMBER_TO_CHANNEL(9)
#define ADC10 ADC_NUMBER_TO_CHANNEL(10)
#define ADC11 ADC_NUMBER_TO_CHANNEL(11)
#define ADC12 ADC_NUMBER_TO_CHANNEL(12)
#define ADC13 ADC_NUMBER_TO_CHANNEL(13)
#define ADC14 ADC_NUMBER_TO_CHANNEL(14)
#define ADC15 ADC_NUMBER_TO_CHANNEL(15)
extern const uint8_t NUM_ADC_CHANNELS;

// ----------- My devices -----------------------
extern SWITCH button;
extern SEGLED led_display;
extern MARQUEE marquee;
#define Temp_sensor1 ADC0
#define Temp_sensor2 ADC2
#define Bat_monitor ADC4
extern LED led_ext1;
extern LED led_ext2;
extern LED leds_int;
extern SABERTOOTH_MOTOR motor_1;
extern SABERTOOTH_MOTOR motor_2;
extern SABERTOOTH_DRIVER Sabertooth;
#define Remoteswitch_relay L6
extern SERVO servo;
extern SERVO_DRIVER Servo;
#define Xbee5v E6

void initHardware(void);
#ifdef __cplusplus
}
#endif
#endif

// undefine all ports so the user cannot change them directly
#undef PORTA
#undef DDRA
#undef PINA
#undef PORTB
#undef DDRB
#undef PINB
#undef PORTC
#undef DDRC
#undef PINC
#undef PORTD
#undef DDRD
#undef PIND
#undef PORTE
#undef DDRE
#undef PINE
#undef PORTF
#undef DDRF
#undef PINF
#undef PORTG
#undef DDRG
#undef PING
#undef PORTH
#undef DDRH
#undef PINH
#undef PORTJ
#undef DDRJ
#undef PINJ
#undef PORTK
#undef DDRK
#undef PINK
#undef PORTL
#undef DDRL
#undef PINL

// Undefine timer registers to stop users changing them
#undef TCNT0
#undef TCCR0B
#undef OCR0A
#undef OCR0B
#undef TCNT1
#undef TCCR1B
#undef OCR1A
#undef OCR1B
#undef OCR1C
#undef TCNT2
#undef TCCR2B
#undef OCR2A
#undef OCR2B
#undef TCNT3
#undef TCCR3B
#undef OCR3A
#undef OCR3B
#undef OCR3C
#undef TCNT4
#undef TCCR4B
#undef OCR4A
#undef OCR4B
#undef OCR4C
#undef TCNT5
#undef TCCR5B
#undef OCR5A
#undef OCR5B
#undef OCR5C


At first the status LED is solid (constantly lit), but when I send a command from the PS2 controller, the Status LED blinks so it knows something is going through.  Admin and others, What am I doing wrong??  There must be a small thing Im missing.

Wiring Info
I have my sabertooth, S1 connected to T (Uart 2) on Axon II

I have my Xbee connected to Uart0 on Axon II

[Admin]

First thing I did wrong, was I sent 0 as triangle, but my motors still should've turned regardless.

I have no idea what sending '0 as triangle' means lol

Is there a way to look at the output of a UART (Sabertooth) port?  I can use an FTDI cable, cant I?

yeap


Rather than giving you the answer, I'll tell you how to debug software like this. What you need to do is add output data that you can see.

Add in rprintf commands here and there in your code, and then view it with Hyperterminal using your FTDI cable. For example:

Code: [Select]

rprintf("\n motorRCurVal is: %d", motorRCurVal);
delay_ms(100);//use this so it doesn't output a billion times per second
You can also use the numbered LED to show a certain number when it's running a certain subset of code. This lets you know what the software is doing behind the scenes.

And just skimming through your code I noticed this very serious bug:
int motorRTarVal;
motorRTarVal=192+motorInt;

The int type only ranges from −128 to 127, so having 192 + anything means your variable is going out of bounds.

[Mastermime]

Thanks for the response

I have no idea what sending '0 as triangle' means lol

Ah sorry.  I forgot to explain.  When I press triangle on the PS2 controller, it sent 0.

yeap


Rather than giving you the answer, I'll tell you how to debug software like this. What you need to do is add output data that you can see.

Add in rprintf commands here and there in your code, and then view it with Hyperterminal using your FTDI cable. For example:

Ok I will do this and report back tomorrow night.

You can also use the numbered LED to show a certain number when it's running a certain subset of code. This lets you know what the software is doing behind the scenes.

And just skimming through your code I noticed this very serious bug:
int motorRTarVal;
motorRTarVal=192+motorInt;

The int type only ranges from −128 to 127, so having 192 + anything means your variable is going out of bounds.

I dont even know what I was thinking...

[Admin]

I prefer to use these types to avoid silly mistakes:

int8_t
uint8_t
int16_t
uint16_t
int32_t

etc. etc.

It's also more compatible with other compilers, as I know of some that consider an 'int' as 16 bit.

[jwatte]

The int type only ranges from −128 to 127, so having 192 + anything means your variable is going out of bounds.

Which compiler are you using?

Using avr-gcc (which is also used by Arduino) the sizes are:

char, unsigned char: 1 byte [-128,127] [0,255]
short, unsigned short, int, unsigned int, pointer: 2 bytes [-32768,32767] [0,65535]
long, unsigned long, float, double: 4 bytes [-2-bazillion,2-bazillion] [0,4-bazillion] [float-range-is-really-large]

[Admin]

I'm using avr gcc. An int is 8 bits. A long int is 16 bits.

I don't use Arduino, but perhaps their library does it differently? The compilers I used for C++ on my PC would make an int as 16 bits and a long int as 32 bits.

The reason I prefer using int8_t (etc.) is that it prevents this confusion.

[Mastermime]

Ah ok.  I just hooked up my ftdi cable to my corresponding Xbee on the controller because in my code it says send rprintf to XbeeSendByte.  It says "value received: -17404" Where the heck is that number coming from and how can I fix it?  I should probably set my rprintf output to UART3 because Xbees are half duplex, therefore how can it transmit and receive data at the same time, right?

Updated code

Code: [Select]

#include "hardware.h"

#define XBee_Controlled
//#define USB_Controlled

#define UART0_TX_BUFFER_SIZE 2
#define UART0_RX_BUFFER_SIZE 2

#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

uint16_t motorLCurVal;
uint16_t motorRCurVal;
uint16_t motorLTarVal;
uint16_t motorRTarVal;
uint16_t motorVal;



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



#ifdef XBee_Controlled
rprintfInit(XbeeSendByte);
#endif


}


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

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

uint16_t tempbyte = NULL;

#ifdef XBee_Controlled
//tempbyte=XbeeGetByte;
if(!uartReceiveBufferIsEmpty(UART0)) {
tempbyte = uartGetByte(UART0);
rprintf(“Value recieved is: %d \n”);
delay_ms(200);
    }
#endif

if (tempbyte != NULL) {

if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
{
setMotorSpeed(tempbyte);
}
else if (tempbyte==START)
{
//pin_low(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(uint16_t 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);
}
}



[Admin]

Ah ok.  I just hooked up my ftdi cable to my corresponding Xbee on the controller because in my code it says send rprintf to XbeeSendByte.  It says "value received: -17404" Where the heck is that number coming from and how can I fix it?

Look at your rprintf statement:
rprintf(“Value recieved is: %d \n”);
and then look at mine:
rprintf("\n motorRCurVal is: %d", motorRCurVal);

Notice something missing?

I should probably set my rprintf output to UART3 because Xbees are half duplex, therefore how can it transmit and receive data at the same time, right?

You can't transmit and receive at the same time with half duplex. Your code will need to allow for both sides to take turns. You could have it transmit only 3 seconds of data, and then stop transmitting again until it's received a new message from the other side. Many ways to do it . . .

[Mastermime]

Code: [Select]

Look at your rprintf statement:
rprintf(“Value recieved is: %d \n”);
and then look at mine:
rprintf("\n motorRCurVal is: %d", motorRCurVal);

Ah whoops.  rprintf(“\n tempbyte is: %d”, tempbyte);


Ok I'm getting the output Axon initiated (just put that in code), but I am getting no data from Xbees in Hyperterminal

I cannot figure out whats going on

[Mastermime]

Ok I'm making progress now.  Disregard that last post. I made a dumb mistake.


I'll take you through exactly what just happened

I turned my controller on (xbee, ps2, arduino)
I connected FTDI cable to uart3
I turned robot on.  Nothing happens as expected.  'Axon Initiated' pops up in Hyperterminal
I move joysticks. It says 'WebbotLib Error:11' and the right motor starts slowly moving (if your looking at robot from front)
As I move the joystick, no matter where I go it displays tempbyte is: 49' sometimes peaks up to 56.

I looked up webbotlib error: 11 and couldn't find anything

Note:  I just tested again and the Webbotlib error: 11 and tempbyte: 49 come up no matter what is pressed, not just joysticks

[Admin]

https://www.google.com/search?q=%22webbotlib+error%3A+11%22

 


http://www.societyofrobots.com/robotforum/index.php?topic=11499.msg87161#msg87161

[Mastermime]

I guess it would've helped to put quotes around it.  lol

So according to General Admin

Error 11 is a receive buffer overflow (see the errors.h file in WebbotLib). This happens when you send a bunch of data but the mcu doesn't process it before the buffer overflows.

Try increasing buffer size in Project Designer. If your baud isn't matching, the 'random junk' can quickly fill a small buffer. Have your mcu repeat back what it sees to verify baud (if you aren't sure).

Yeah right now I have my receive buffer and transmit buffer on the Xbee set to zero.  What should I set it to and also should i set a buffer on the other UART ports as well?

Last question, currently my baud rate is at 9600,  would increasing my baud rate help too?

[Admin]

Yeah right now I have my receive buffer and transmit buffer on the Xbee set to zero.  What should I set it to and also should i set a buffer on the other UART ports as well?

I usually set my buffers to 80/80. But if you are receiving data faster than you are reading it, you'll always overflow in the long term.

Last question, currently my baud rate is at 9600,  would increasing my baud rate help too?

Definitely. Faster the better.

Also, if your while loop is sending data infinitely fast, that will also do it. Add in a small delay of a few ms if needed.

[Mastermime]

Ok here is my updated code with the buffers set to 100.  The baud rates are set 38400

Code: [Select]

#include "hardware.h"

#define XBee_Controlled
//#define USB_Controlled

#define UART0_TX_BUFFER_SIZE 80
#define UART0_RX_BUFFER_SIZE 80

#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

uint16_t motorLCurVal;
uint16_t motorRCurVal;
uint16_t motorLTarVal;
uint16_t motorRTarVal;
uint16_t motorVal;



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



#ifdef XBee_Controlled
rprintfInit(uart3SendByte);
delay_ms(100);
#endif


}


// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
rprintf("\nAxon Initiated.\n\n");
motorLCurVal = motorLTarVal = MOTOR_L_STOP;
motorRCurVal = motorRTarVal = MOTOR_R_STOP;
return 0;
}

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

uint16_t tempbyte = NULL;

#ifdef XBee_Controlled
//tempbyte=XbeeGetByte;
if(!uartReceiveBufferIsEmpty(UART0)) {
tempbyte = uartGetByte(UART0);
rprintf(“\n tempbyte is: %d”, tempbyte);
delay_ms(200);
      }
#endif

if (tempbyte != NULL) {

if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
{
setMotorSpeed(tempbyte);
}
else if (tempbyte==START)
{
pin_low(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(uint16_t 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);
}
}

I still get an output ranging from 48-55 no matter where I press on the playstation (triangle, start, joysticks).   No idea what this signifies.

[Admin]

You need more rprintf's.

Add them to say what button you pushed, what if statement has been accessed, whenever a variable is modified, etc. - literally everywhere.

This will help you narrow down the problem.

[Mastermime]

Ok here is my updated code with more printfs

Code: [Select]

#include "hardware.h"

#define XBee_Controlled
//#define USB_Controlled

#define UART0_TX_BUFFER_SIZE 80
#define UART0_RX_BUFFER_SIZE 80

#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

uint16_t motorLCurVal;
uint16_t motorRCurVal;
uint16_t motorLTarVal;
uint16_t motorRTarVal;
uint16_t motorVal;



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



#ifdef XBee_Controlled
rprintfInit(debugSendByte);
delay_ms(100);
#endif


}


// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
rprintf("\nAxon Initiated.\n\n");
motorLCurVal = motorLTarVal = MOTOR_L_STOP;
motorRCurVal = motorRTarVal = MOTOR_R_STOP;
return 0;
}

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

uint16_t tempbyte = NULL;

#ifdef XBee_Controlled
//tempbyte=XbeeGetByte;
if(!uartReceiveBufferIsEmpty(UART0)) {
tempbyte = uartGetByte(UART0);

delay_ms(200);
    }
#endif

if (tempbyte != NULL) {

if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
{
setMotorSpeed(tempbyte);
rprintf(“\n motorspeed is: %d”, tempbyte);
}
else if (tempbyte==START)
{
pin_low(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;
}

//code below by Tzankoff
//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);
rprintf("\n StopmotorLCurVal is: %d", motorLCurVal);
delay_ms(100);

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

Sabertooth_uartSendByte(motorRCurVal);
rprintf("\n StopmotorRCurVal is: %d", motorRCurVal);
delay_ms(50);

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

void setMotorSpeed(uint16_t byte)
{
if (byte >= MOTOR_L_MIN && byte <= MOTOR_L_MAX)
{
motorLTarVal = byte;
do {
if (motorLCurVal < motorLTarVal)
{
Sabertooth_uartSendByte(++motorLCurVal);
rprintf("\n setSpeed++motorLCurVal is: %d", ++motorLCurVal);

}
else
{
Sabertooth_uartSendByte(--motorLCurVal);
rprintf("\n setSpeed--motorLCurVal is: %d", --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);
rprintf("\n setSpeed++motorRCurVal is: %d", ++motorRCurVal);

}
else
{
Sabertooth_uartSendByte(--motorRCurVal);
rprintf("\n setSpeed--motorRCurVal is: %d", --motorRCurVal);

}
delay_ms(100);

} while (motorRCurVal != motorRTarVal);
}
}

Then here is the output when I tested

Code: [Select]

Axon Initiated.               

 setSpeed--motorLCurVal is: 62                             
 setSpeed--motorLCurVal is: 60                             
 setSpeed--motorLCurVal is: 58                             
 setSpeed--motorLCurVal is: 56                             
 setSpeed--motorLCurVal is: 54                             
 setSpeed--motorLCurVal is: 52                             
 setSpeed--motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal                       
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal                       
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal                       
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal                       
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal                       
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal                       
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50WebbotLib Error:11                                               

 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50                             
 setSpeed--motorLCurVal is: 48                             
 setSpeed++motorLCurVal is: 50So obviously, there is something wrong the setMotorSpeed part.  By the way, instead of "part" whats the proper term for that.  I've never taken a programming class?

As I mentioned earlier, We got the code from Tzankoff and he included a section (below) that my friend (programmer) that I am working with is not sure we should include.  Here is what my friend says

Code: [Select]

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

}
*/
So my argument is, if this is part of Tzankoffs final code, it must've worked.  Is my friends logic (his explanation at top) correct and should I include this code?  I'm having trouble following it.  Also I forgot to mention when you said  192 was out of bounds, that part was in this commented out section.

[Webbot]

I gather (via email) that you have sorted this out. Some info may help others ....

RE "I get WebbotLib Error 11 and I don't know what it means..." - first look in 'errors.h' to get a bit more info. Don't forget you've got the src code for WebbotLib so you could find exactly why you get the error.

@Admin jwatte is 100% correct. An int is 16 bits on all avr compilers ie an int16_t.

Edit:
I also note that driveSpeed you are setting is very specific to the Sabertooth - ie you are assuming the sabertooth is running in 'simple' mode where motor1: 1=full reverse,64 is stop,127 is full fwd and for motor2: 128 is full reverse,192 is stop, 255 is full fwd.
I see that by your defines:

Code: [Select]

#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

Seems you are trying to convert Arduino code that only works with the Sabertooth into WebbotLib code?
It doesn't work like that.
In WebbotLib  you set a drive speed from -127 (full reverse), 0=stop, +127=full fwd and WebbotLib turns it into the relevant numbers that are sent to the Sabertooth.
Unlike Arduino this means you can swap the Sabertooth to a different controller and not have to change your code.

[Admin]

@Admin jwatte is 100% correct. An int is 16 bits on all avr compilers ie an int16_t.

hmmm! ok, I stand corrected. I was clearly having a 'senior moment' lol . . .

(Perhaps I was thinking of old PIC compilers? I stopped using 'int' ages ago . . .)

[Mastermime]

Seems you are trying to convert Arduino code that only works with the Sabertooth into WebbotLib code?
It doesn't work like that.
In WebbotLib  you set a drive speed from -127 (full reverse), 0=stop, +127=full fwd and WebbotLib turns it into the relevant numbers that are sent to the Sabertooth.
Unlike Arduino this means you can swap the Sabertooth to a different controller and not have to change your code.

Thanks.  I will definitely keep this in mind later, but first I have to focus on getting the right bytes from XBee. 

I just performed a test and I noticed different patterns when I press different buttons.  I am only looking at the rprintf (added it, not in code though right after tempbyte = uartGetByte(UART0);

I commented the other ones out.  I am having trouble decoding this.


triangle- 50, 53,54
start- 50, 53, 55
right trigger- 50, 53, 57
left trigger- 50, 53, 56
left joystick all the way forward- 49, 50, 55
right joystick all the way forward- 50, 53, 53
right joystick all the way down- 49, 50, 56
left joystick all the way down- 49, 49, 49 (may just be 49, 49 couldnt really tell)

I am having trouble decoding this.

[Admin]

I haven't used the PS controller before, but it looks like you can do something like this:

Code: [Select]

//for triangle
if(variableA == 50 && variableB == 53 && variableC == 54)
{
do_something();
}

[Mastermime]

Thats a great idea that would work for the buttons, but i wouldn't have any speed control for the joysticks.  something isnt right.  Thats not the output I should get

[Mastermime]

What do these commands mean though (a,b,c) ?

Triangle 50,53,54

when it should be 256

This is what its supposed to be

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

This is what it is

triangle- 50, 53,54
start- 50, 53, 55
right trigger- 50, 53, 57
left trigger- 50, 53, 56
left joystick all the way forward- 49, 50, 55
right joystick all the way forward- 50, 53, 53
right joystick all the way down- 49, 50, 56
left joystick all the way down- 49, 49, 49 (may just be 49, 49 couldnt really tell)

The third number in the actual output is 2 less than what it should be
ie 54 is less than [2]56
ie 55 is 2 less than [2]57


Does this have anything to do with it?  Im just looking for patterns

I will do anything if someone can figure out what's going on