Society of Robots - Robot Forum

Software => Software => Topic started by: Mastermime on February 02, 2013, 03:29:40 PM

Title: Controlling motors [Urgent]
Post by: Mastermime on February 02, 2013, 03:29:40 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.  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
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 03, 2013, 02:08:10 PM
Uhh still no luck.  Please help
Title: Re: Controlling motors [Urgent]
Post by: waltr on February 03, 2013, 05:47:18 PM
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.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 03, 2013, 06:16:27 PM
Thanks a lot for the response.  I really appreciate it

Quote
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

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

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

Quote
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. 
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 04, 2013, 10:37:21 PM
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
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 05, 2013, 11:16:09 AM
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   :P

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.

Quote
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?
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 05, 2013, 07:31:50 PM
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
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 05, 2013, 11:48:18 PM
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?
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 06, 2013, 05:47:46 PM
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
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 06, 2013, 09:02:32 PM
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

Quote
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. :P
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 06, 2013, 09:16:35 PM
Thanks for the response

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

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


Quote
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...
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 06, 2013, 09:21:00 PM
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.
Title: Re: Controlling motors [Urgent]
Post by: jwatte on February 07, 2013, 11:03:08 AM
The int type only ranges from −128 to 127, so having 192 + anything means your variable is going out of bounds. :P

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]

Title: Re: Controlling motors [Urgent]
Post by: Admin on February 07, 2013, 12:18:46 PM
I'm using avr gcc. An int is 8 bits. A long int is 16 bits. :P

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.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 07, 2013, 09:54:10 PM
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);
}
}

Title: Re: Controlling motors [Urgent]
Post by: Admin on February 07, 2013, 10:51:51 PM
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? :P

Quote
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 . . .
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 08, 2013, 01:00:15 AM
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


Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 08, 2013, 12:55:56 PM
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
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 08, 2013, 01:30:29 PM
https://www.google.com/search?q=%22webbotlib+error%3A+11%22 (https://www.google.com/search?q=%22webbotlib+error%3A+11%22)

 :P


http://www.societyofrobots.com/robotforum/index.php?topic=11499.msg87161#msg87161 (http://www.societyofrobots.com/robotforum/index.php?topic=11499.msg87161#msg87161)
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 08, 2013, 02:00:17 PM
I guess it would've helped to put quotes around it.  lol

So according to General Admin
Quote
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?
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 08, 2013, 02:07:03 PM
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.


Quote
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.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 08, 2013, 04:47:43 PM
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.
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 08, 2013, 04:57:02 PM
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.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 08, 2013, 11:21:57 PM
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: 50
So 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.
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 09, 2013, 09:46:46 PM
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.

Title: Re: Controlling motors [Urgent]
Post by: Admin on February 10, 2013, 07:45:53 AM
Quote
@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 . . .)
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 04:39:44 PM
Quote
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.


Title: Re: Controlling motors [Urgent]
Post by: Admin on February 10, 2013, 05:04:39 PM
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();
}
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 05:11:00 PM
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
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 06:57:41 PM
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
Quote
#define TRIANGLE 256
#define START 257
#define L2 258
#define R2 259

This is what it is
Quote
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
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 10, 2013, 09:01:35 PM
Can you post the block of code that outputs those values?

And please please comment your code. It's hard to understand what's going on. :P

read through a few of these posts:
https://www.google.com/search?q=good+code+commenting+rules (https://www.google.com/search?q=good+code+commenting+rules)

ps - It's better to post just the relevant code, instead of ALL of it, because it makes it easier for us to just zoom in on the important stuff. But don't abbreviate your code too much, because then we might not see the mistake right next to it ;D
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 09:25:50 PM
Ok sorry about that.  I thought I commented it enough.  Here is the block of code I BELIEVE is incorrect.  I have tested the Xbees and they are working properly
Code: [Select]
#include "hardware.h"

#define XBee_Controlled

#define UART0_TX_BUFFER_SIZE 80
#define UART0_RX_BUFFER_SIZE 80


// Initialize the hardware
void appInitHardware(void) {

initHardware();  //Initialize all hardware including uarts

#ifdef XBee_Controlled
rprintfInit(debugSendByte);  //Send output to computer through FTDI

delay_ms(100);
#endif
rprintf("\nHardware initialized.\n\n"); 
}


// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
rprintf("\nAxon Initiated.\n\n");

}

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

uint16_t tempbyte = NULL;

#ifdef XBee_Controlled
if(!uartReceiveBufferIsEmpty(UART0)) {
tempbyte = uartGetByte(UART0);  //Get byte from Xbee
rprintf(“\n tempbyte is: %d”, tempbyte);  //Tempbyte is __
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
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;
}

I just put all the code in there before so it's all there in case you need to look somewhere
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 10, 2013, 09:47:31 PM
A few notes:

You can define this in Project Designer, in the UART:
Code: [Select]
#define UART0_TX_BUFFER_SIZE 80
#define UART0_RX_BUFFER_SIZE 80

Use %u for uint16, not %d (which is for signed).
Code: [Select]
rprintf(ā€œ\n tempbyte is: %dā€, tempbyte);
. . . but that shouldn't be the source of your problems.

This is your setup, right?

PS controller -> arduino -> Xbee -> the other Xbee -> Axon -> USB -> hyperterminal

Is it possible the Arduino isn't outputting the correct data?
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 09:55:08 PM
Quote
Use %u for uint16, not %d (which is for signed).

I will change that


Quote
PS controller -> arduino -> Xbee -> the other Xbee -> Axon -> USB -> hyperterminal
I think this is what you mean but Axon (UART3) > ftdi > hypertrm

Quote
Is it possible the Arduino isn't outputting the correct data?
I dont think so.  I checked the output from the xbee on the robot and it is correct

Quote
25625625625612752525252525252525252525252511127891271271271271271271271271279294127127127127127127127127127127
I dont have to say new line everytime I send a byte right?  I dont think so.  Could it be my UART port that is not working (unlikely)?
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 10, 2013, 10:02:33 PM
Quote
Is it possible the Arduino isn't outputting the correct data?
I dont think so.  I checked the output from the xbee on the robot and it is correct
Can you post here the output? Or is that the example below?

Quote
25625625625612752525252525252525252525252511127891271271271271271271271271279294127127127127127127127127127127
I dont have to say new line everytime I send a byte right? Could it be my UART port that is not working (unlikely)?
You don't need a new line each time. But for debugging purposes, could you add a space between each value transmitted? I can't tell if you are sending a '56', or a '25', etc.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 10:08:56 PM
Quote
Can you post here the output? Or is that the example below?
The first few are:
256 
256
256
127
127

Split up in threes at first
Should be adequate enough
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 10, 2013, 10:19:00 PM
Well, if I break it up into 3's I get this:
256 256 256 256 127 525 252

Do you see the 525 in there? That looks suspicious to me . . .

Can you post the block of code that transmits these values on the Arduino to the Xbee? (and also the part where you printed out those numbers above?)
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 10, 2013, 10:31:48 PM
I cant remember exactly, but I think I may have touched a joystich which either read as 52 or 5.  I cant remember.  I know that the Xbees are properly sending bytes though.  I wouldn't waste your time on that.  I dont have the output with lines inbetween anymore either.  I have tested that multiple times though.

How can something so simple (receiving bytes) go so wrong?   >:(
Title: Re: Controlling motors [Urgent]
Post by: newInRobotics on February 11, 2013, 11:09:07 AM
Did you check baud rates of both devices? Do they match? Did You check that selected baud rates match well with respective clock to produce lowest error possible?
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 11, 2013, 03:37:28 PM
Quote
Did you check baud rates of both devices? Do they match?

I just checked The baud rates and they match and are set at 38400.

Quote
Did You check that selected baud rates match well with respective clock to produce lowest error possible?

No I did not.  I just set the highest baud rate I could.  This wouldnt be the source of my problem, would it?




Admin:  I dont think that is the problem.  Let me reiterate, for some reason the Axon is taking the data and twisting into three different numbers depending on the command I press.

example when i press triangle
tempbyte is: 50
tempbyte is: 53
tempbyte is: 54

Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 11, 2013, 05:06:57 PM
I just noticed something.  I dont have anything connected to the ground port on UART.  Should that be connected to common?

EDIT: That wasn't it
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 11, 2013, 06:12:49 PM
I guess I'm being a bit dump (never used XBee etc) but you talk in your original post about 'PS2 controller over XBee'. Hence I'm not sure what s/w is running on the cpu - ie is it using WebbotLib PS/2 controller code or is the whole XBee thing just sending a serial command to a UART which you are trying to make sense of? If its the latter then connecting straight into HyperTerminal, or others, would show would what's being received.

As always - a copy of your .prj file might help (me!) or at least the PDF output from ProjectDesigner (menu option File|Print).
Apologies if it seems dumb - but don't forget like Admin asked re code commenting - you have lived/breathed this project and know what is what and how it all hangs together. We are totally blind and so you must bear with us and help us to 'see' your set up in detail .

For example when you say:
Quote
Code: [Select]
This is what its supposed to be

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


Who says so? Where do these values come from?

Sorry - just trying to go back to first principles.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 11, 2013, 06:24:11 PM
Quote
is the whole XBee thing just sending a serial command to a UART which you are trying to make sense of? If its the latter then connecting straight into HyperTerminal, or others, would show would what's being received.

This is correct.  I connected UART3 to Hyperterminal (where I sent my rprintf).  But thats where Im getting numbers like 50, 53, 54.  I will post the output I get directly from the Xbee receiver on the robot so you guys can see

Quote
Apologies if it seems dumb - but don't forget like Admin asked re code commenting - you have lived/breathed this project and know what is what and how it all hangs together. We are totally blind and so you must bear with us and help us to 'see' your set up in detail .

Oh I understand completely.  That's why I included all my code at first.

Quote
Who says so? Where do these values come from?

Sorry - just trying to go back to first principles.

Thats what I defined "When I press triangle, send 256" in my Arduino code

Arduino Output directly from Xbee on Robot (receiver)
Quote
25625625625625625625625625625625625625625625625625625625625625625625625625625625
62572572572572572572572572572572572572572572572572572572572572572572572572572572
59259259259259259259259259259259259259259259259259259259259259259259259259259259
25825825825825825825825825825825825825825825825825825825825825825825825825825825
82092552552552552552552552552552552552552552552552552552552552552552552552552552
10188128128128128128128128128128128128128128128128128128128128128128128128128128
12712712712712712712712712712712712712712712712712712712712712712712712712712752
461111111111111111111111111111111111111111111111111111111111111111111111111111
first line: 256 triangle
second line:257 start
next two lines: left trigger and right trigger
rest of lines: joystick commands
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 11, 2013, 08:18:33 PM
Ok think I'm getting there....

Quote
example when i press triangle
tempbyte is: 50
tempbyte is: 53
tempbyte is: 54

If you look at the ascii codes (http://www.asciitable.com/ (http://www.asciitable.com/)) for 50,53, 54 then you get '2', '5', '6' respectively.
So your sender is sending ascii characters but you are printing them out in decimal.

you could probably prove this by changing your printf to print them as characters instead ( note the %c, the '(char)' is probably optional)
Code: [Select]
rprintf(ā€œ\n tempbyte is: %cā€, (char)tempbyte);  //Tempbyte is __

So for '256' you are receiving three separate bytes/chars. You will have to write code on your receiver to parse it back into a number (int say).
Hopefully your sender either has a delimiter between numbers, or always uses 3 character bytes, otherwise you wont know when one number ends and the next begins. eg is
"256123456"
parsed as 256, 123, 456 or as 256,12, 345,6 etc etc

-- edit --
Alternatively you could change the Arduino code to send them as numbers, not text, but then you could only use numbers in the range 0 to 255 (to fit into a byte)
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 11, 2013, 10:22:30 PM
Wow...  I cannot believe this is finally solved.  Thank you sir so much.  You are a genius!  How can I repay you?   I will test and let you guys know how everything goes tomorrow   :D :D :D :D :D
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 12, 2013, 11:56:00 AM
Quote
If you look at the ascii codes (http://www.asciitable.com/ (http://www.asciitable.com/)) for 50,53, 54 then you get '2', '5', '6' respectively.
I was suspecting the problem to be the Arduino code, too, but I wouldn't have figured the above out given the available information in a million years lol  :o


Referring to other questions . . . if it was the wrong baud rate, you'd get random crazy characters, or none at all. As for ground, you need *a* ground connected. I included one for each UART to serve as a reminder and as an extra pin. But any ground on the Axon is ok. If ground wasn't connected, you'd like not get any data at all.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 12, 2013, 01:11:53 PM
Quote
if it was the wrong baud rate, you'd get random crazy characters, or none at all.  As for ground, you need *a* ground connected. I included one for each UART to serve as a reminder and as an extra pin. But any ground on the Axon is ok. If ground wasn't connected, you'd like not get any data at all.

Yeah that's what I thought would happen with the bauds and grounds.  Thanks a lot for your help Admin.  Much appreciated


Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 12, 2013, 01:36:54 PM
The other thing I noticed in your project was an output pin E5 called 'Xbee5v'. I presume you are not using this to provide power to the XBee ? That could be bad and blow the pin, or worse, if the XBee needs more power than the cpu can provide. Either use a 5v regulated supply pin or, if you want to turn supply to the XBeee on and off, then you should use the pin to control either a relay or a MOSFET to do it.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 12, 2013, 01:40:45 PM
Yeah I'm not using E5.  This was a mistake.  I am supplying the Xbee with a regulated 5v.  I should delete that
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 13, 2013, 06:09:19 PM
Everything is working great!   :D :D

EDIT: but i still get occasional buffer overflow and then everything goes haywire.  I took out all my rprintfs.  Should I increase the buffer to 120 or 130?  Can I do this in my code or do I have to go back into Project Designer and do it?  Right now I have it set in my c file and in pd to 80/80
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 13, 2013, 08:01:14 PM
You just need to slow down your rprintf's and use the max baud rate. Adding small delays usually does the job for me.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 13, 2013, 08:14:11 PM
Ok I commented out all my rprintfs so that shouldnt be a factor.  Small delays in the Arduino code right?  Yeah I'm doing a bit of research.  I dont understand buffers completely, but I notice a lot of people have like 20/40.  I'm using the max baud rate of 38400.
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 13, 2013, 08:17:35 PM
Buffer overflow errors are from either receiving or sending data by UART too fast. If your Arduino is sending too fast, slow it down, etc.

Adding a simple delay_ms(10); in your main while loop might be all that's needed. Or if the Arduino, however they do it.

Ok I commented out all my rprintfs so that shouldnt be a factor.  Small delays in the Arduino code right?  Yeah I'm doing a bit of research.  I dont understand buffers completely, but I notice a lot of people have like 20/40.  I'm using the max baud rate of 38400.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 13, 2013, 08:37:56 PM
oh ok thats what I was thinking.  Thanks
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 14, 2013, 12:30:31 PM
The reason it goes 'mad' is because, due to the overflow, you have missed one or more characters and so your parsing routine probably gets confused. Its usually a good idea to have some sort of delimiter between the number (ie spaces or commas say) - your parsing routine can then reset itself when it receives a delimiter to expect the start of a new number.

Other alternatives are to not constantly keep xmiting the same info  from the Arduino. eg don't keep saying that a button is down/up - just send a msg when the button is pressed and then another when it is released.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 17, 2013, 04:14:45 PM
Awesome! I just added the delimiter and my problem is solved.  thanks Webbot.

Actually I just ran into a problem.  For some reason, I cannot run both motors at a time.  Check out the video to see what I mean.


VoltX Wireless Test 5 (http://www.youtube.com/watch?v=_kM3gEjvV6M#ws)

Code I was using

Code: [Select]
#include "hardware.h"

#define XBee_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;

boolean receiving;
#define PACKET_SIZE 3
char packet[] = {0, 0, 0};
int curByte;

// Initialize the hardware
void appInitHardware(void) {

initHardware();

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

//rprintf("\nHardware initialized.\n\n");
}


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

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

uint16_t tempbyte = NULL;

#ifdef XBee_Controlled
if(!uartReceiveBufferIsEmpty(UART0)) {
char temp = uartGetByte(UART0);
if (temp == 'A' && !receiving)  //delimiter
{
receiving = true;
curByte = 0;
}
else
{
packet[curByte] = temp;
//rprintf(“Got a byte! %c\n”, temp);
if (curByte == (PACKET_SIZE - 1) )
{
tempbyte = atoi(packet);  //atoi = convert string to packet
receiving = false;
}
curByte++;
}
}
#endif
   

if (tempbyte != NULL) {

if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_R_MAX)
{
setMotorSpeed(tempbyte);
}
else if (tempbyte== START)
{
pin_toggle(Remoteswitch_relay);
}
else if (tempbyte== TRIANGLE)
{
pin_toggle(led1);
pin_toggle(led2);
pin_toggle(led3);
}

tempbyte = NULL;
}
else
{
motorStopMotor();
}


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


//All the ++ and -- is just to make the robot gradually stop and start
//motorXCurVal is the current speed.
//motorXTarVal is where we want the speed to be.

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

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

Sabertooth_uartSendByte(motorRCurVal);
delay_ms(10);

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

} 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(10);

} while (motorRCurVal != motorRTarVal);
}
}

Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 18, 2013, 09:07:37 PM
Just looking at your code:-
The atoi function looks at data in packet - but suppose you've just written a '6' and '4' in there. The answer to atoi will be 64? Not always- if it previously had '256' then it will now have '646'. The '64' being new and the trailing '6' being old stuff.

So you need to make sure that since 'packet' is being treated as a string that you write a zero terminator in there.
change
Code: [Select]
char packet[] = {0, 0, 0}; to
Code: [Select]
char packet [PACKET_SIZE+1] to allow for this extra 'end of string char'

Then change:
Code: [Select]
packet[curByte] = temp; to add the end of string character ie
Code: [Select]
packet[curByte] = temp; packet[curByte+1]='\0';
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 18, 2013, 11:41:39 PM
You would be correct if I didn't buffer every number with 0's. but like for 64, I actually send 064. So it always is full with the correct #. You wouldn't know this b/c its on the arduino side of things. Not thats its bad practice, but thats not our problem for the packet response. Helpful though.  Sorry I should've mentioned that. 

When this odd phenomenon occurs, the red led on the Sabertooth lights up.  I dont think this is the source of my problem because I believe that lights due to high startup current.  This is very odd.  I've had a lot of odd problems!
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 19, 2013, 12:15:44 AM
Quote
I dont think this is the source of my problem because I believe that lights due to high startup current.
To verify the hardware isn't the problem, write a simple program that commands both motors to run together.
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 19, 2013, 11:59:54 AM
You would be correct if I didn't buffer every number with 0's. but like for 64, I actually send 064. So it always is full with the correct #. You wouldn't know this b/c its on the arduino side of things. Not thats its bad practice, but thats not our problem for the packet response. Helpful though.  Sorry I should've mentioned that. 

Yes - but you are storing the three digits in memory and the 'atoi' function keeps on going until it finds something that is not a digit. ie 'atoi' works with a string not an array of characters. A string is the same as an array of characters except that it has an extra zero at the end to makrk the end of the string.

You probably have no idea what data is living in memory immediately after the 3 'packet' bytes.
For example: if you had
char packet[3] = { '0','0','1'} and this was followed in memory by something else say like:
char foobar[] = { '3', '2', '1'}
then when you do 'atoi(packet)' you would get back 1321 which is probably not what you expect.

Even worse foobar could be a variable and so the atoi may sometimes do what you want and at other times not.

Don't forget - only the compiler know that 'packet' is 3 digits long - at runtime the 'atoi' function just takes its first byte to be the start of a, potentially, infinitely long list of characters.

To fix your code then you MUST declare packet as 4 bytes rather than 3 and make sure that the last byte is a zero (ie the number 0 not the character '0' - ie you can use packet[3]=0 or packet[3]='\0')


Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 21, 2013, 12:26:37 AM
Ok I will integrate this into my code tomorrow after I get home from school.  Do you think this is the problem though for the odd motor occurrences?  I forgot to just set the motors at 256 to see if its a hardware problem.  I will do this as well.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 21, 2013, 09:59:32 PM
Ok I set the motors at half speed and they worked.

I added the change Webbot suggested and the same results occurred.  It's almost as if the Sabertooth can't interpret data at the same time.  I'm checking my data (rprintf) and I'm getting the correct data from the Xbee when I push both forward
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 25, 2013, 05:43:30 PM
Ahh man I tried another thing, but that didn't work either. 
Title: Re: Controlling motors [Urgent]
Post by: Admin on February 25, 2013, 09:05:15 PM
Whenever I get stuck like this, I just comment out all of the code except the most important parts.

I also simplify everything to the very basics.

Then verify each part of the code in small chunks.
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 25, 2013, 09:58:02 PM
Going back to one of my earlier posts - are you still sending Sabertooth commands yourself (ie in ProjectDesigner the Sabertooth is just a UART) or are you then trying to send them via WebbotLib to set the speed (ie in ProjectDesigner the Sabertooth is a Sabertooth).

Title: Re: Controlling motors [Urgent]
Post by: Mastermime on February 26, 2013, 02:13:56 PM
Well I set up Sabertooth in project designer as a Sabertooth, but I'm sending commands 0-255 not the Webbotlib -64 - 127.  I should probably change that to see if that may be part of the problem.
Title: Re: Controlling motors [Urgent]
Post by: Webbot on February 26, 2013, 04:10:17 PM
Ok - here are the steps that I would take:-

1-Get the joystick controller wired up to Hyperterminal (or other).  For each motor/stick make sure that it is sending 0 (ie in your case '000') for full reverse and '255' for full forward - irrespective of the fact that one motor is probably needing to turn the opposite way (we fix that in WebbotLib).

2-Now use your existing code to scan that 3 char string back into a number from 0 to 255. You could, as an interim, just dump these values to the USB/UART to make sure they are being received ok.

3-Now introduce the Sabertooth via Project Designer. Make one of the motors 'inverted' so that it turns the opposite way to the other if needs be. But before you can send it a command via WebbotLib you need to map the incoming values of 0-255 into the drive speed range DRIVE_SPEED_MIN to DRIVE_SPEED_MAX. You can do this easily with:-
Code: [Select]
uint8_t received = the number from 0 to 255 that you received
DRIVE_SPEED speed = interpolate(received, 0, 255, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
4. Now that you've got a DRIVE_SPEED you can use it to call act_setSpeed on the Sabertooth motor.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on March 06, 2013, 09:44:39 PM
Ok I just got back to this.  I believe I have achieved step 1 and 2, but the easiest step, step 3 is causing me problems.   I am just trying to set the speed initially with the act_setSpeed function so I made a simplified test file, but when I turn the robot on, I get no activity from the motors.  I'm probably making a really dumb mistake I can't see.

Also just wondering, Why will act_setSpeed differ from what I'm using?  I know that use the same code to drive motors and modified servos, but why will this change anything for my particular problem?

Heres my simple code.

Code: [Select]
#include "hardware.h"

#define XBee_Controlled
#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))

#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

// Initialize the hardware
void appInitHardware(void) {

initHardware();

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

}


// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
}

// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
uint8_t received = XbeeGetByte;
DRIVE_SPEED speed = interpolate(received, 0, 255, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
act_setSpeed(&motor_1, 100);
act_setSpeed(&motor_2, 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;
}
Title: Re: Controlling motors [Urgent]
Post by: Webbot on March 08, 2013, 01:20:45 PM
The act_setSpeed will convert your value of 100 into almost full power forward and send a command out to the uart. Since you are using a constant value it will/should(?) only send it once.

First check that you've got the DIP switches on the Sabertooth set up according to the picture in Project Designer for the Sabertooth - as this has an effect on the commands that are sent out (eg packetized vs simple mode).

Try disconnecting the UART output from the Sabertooth and send it to your PC instead - assuming you've got a TTL to RS232/USB converter - then you can see exactly what cmd is being sent to the Sabertooth.

I'm assuming that if you create a fresh project and just use the default generated example code that the motors do move between full fwd and full reverse every 10 seconds.

Title: Re: Controlling motors [Urgent]
Post by: Mastermime on March 09, 2013, 12:26:31 PM
Ok I just tested again with fresh code generated from Webbotlib

Quote
The act_setSpeed will convert your value of 100 into almost full power forward and send a command out to the uart. Since you are using a constant value it will/should(?) only send it once.

Yep that's exactly what happened so that's good

Quote
First check that you've got the DIP switches on the Sabertooth set up according to the picture in Project Designer for the Sabertooth - as this has an effect on the commands that are sent out (eg packetized vs simple mode).
DIP switches checked and they are correct

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) {

// -------- 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
uint8_t received = XbeeGetByte;

DRIVE_SPEED speed = interpolate(received, 0, 255, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
// Set speed for all motors/servos
act_setSpeed(&motor_1,100);

act_setSpeed(&motor_2,100);

// -------- End   Actuators -------

return 0;
}

So now if I rewrite the receiving code, it should work correct if I use this
Code: [Select]
uint8_t received = the number from 0 to 255 that you received
DRIVE_SPEED speed = interpolate(received, 0, 255, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);

and then use the code below to set it according to the bytes received
Code: [Select]
act_setSpeed(&motor_1,speed);
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on March 16, 2013, 08:09:29 PM
Ok I just tried it with some new code I wrote and I got a horrible result.  Only the left motor moved and it wasn't very responsive. 

Then the worst part was my motor driver went up in flames  :'(.  I couldn't tell if it was the code or if I accidentally shorted something out, but the Sabertooth should have protection against overcurrent and shorting something out.

Is there something noticeably wrong with this code that caused the fire?  It would've been probably easier to adapt the command to my old code.
Code: [Select]
#include "hardware.h"

#define XBee_Controlled
#define DEBUG 1

#define UART0_TX_BUFFER_SIZE 80
#define UART0_RX_BUFFER_SIZE 80

#define ACCELERATION 5
#define DECELERATION 5
#define IDLE_LOOPS_TILL_STOP 10
#define DRIVE_SPEED_MIN -128
#define DRIVE_SPEED_MAX 128

#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

#ifndef max
#define max( a, b ) ( ((a) > (b)) ? (a) : (b) )
#endif

#ifndef min
#define min( a, b ) ( ((a) < (b)) ? (a) : (b) )
#endif

int16_t motorLCurVal;
int16_t motorRCurVal;
int16_t motorLTarVal;
int16_t motorRTarVal;
int16_t motorLIdle;
int16_t motorRIdle;

boolean receiving;
#define PACKET_SIZE 3
char packet[] = {0, 0, 0, 0};
int curByte;

// Initialize the hardware
void appInitHardware(void) {

initHardware();

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

//rprintf("\nHardware initialized.\n\n");
}


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

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

uint16_t tempbyte = NULL;

#ifdef XBee_Controlled
if(!uartReceiveBufferIsEmpty(UART0)) {
char temp = uartGetByte(UART0);
if (temp == 'A' && !receiving)
{
receiving = true;
packet[3] = '\0';
curByte = 0;
}
else
{
packet[curByte] = temp;
if (curByte == (PACKET_SIZE - 1) )
{
tempbyte = atoi(packet);
rprintf("Packet recieved: %d", tempbyte);
receiving = false;
}
curByte++;
}
}
#endif

if (tempbyte != NULL)
{
if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_L_MAX)
{
motorLTarVal = interpolate(tempbyte, 1, 127, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
motorLIdle = 0; motorRIdle++;
}
else if (tempbyte >= MOTOR_L_MIN && tempbyte <= MOTOR_L_MAX)
{
motorRTarVal = interpolate(tempbyte, 128, 255, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
motorRIdle = 0; motorLIdle++;
}
else
{
motorLIdle++; motorRIdle++;
switch(tempbyte)
{
case TRIANGLE:
break;

case START:
break;

case L2:
break;

case R2:
break;

default:
break;
}
}
tempbyte = NULL;
}

adjustMotorSpeed(); //Only moves motor if current value != target value

if (motorLIdle > IDLE_LOOPS_TILL_STOP)
{
motorLTarVal = 0;
}

if (motorRIdle > IDLE_LOOPS_TILL_STOP)
{
motorRTarVal = 0;
}

return 0;
}

void adjustMotorSpeed()
{
if (motorLCurVal < motorLTarVal)
{
motorLCurVal = min((motorLCurVal + ACCELERATION), motorLTarVal);
act_setSpeed(&motor_1, motorLCurVal);
}
if (motorLCurVal > motorLTarVal)
{
motorLCurVal = max((motorLCurVal - DECELERATION), motorLTarVal);
act_setSpeed(&motor_1, motorLCurVal);
}
if (motorRCurVal < motorRTarVal)
{
motorRCurVal = min((motorRCurVal + ACCELERATION), motorRTarVal);
act_setSpeed(&motor_2, motorRCurVal);
}
if (motorRCurVal > motorRTarVal)
{
motorRCurVal = max((motorRCurVal - DECELERATION), motorRTarVal);
act_setSpeed(&motor_2, motorRCurVal);
}

}

Title: Re: Controlling motors [Urgent]
Post by: Webbot on March 17, 2013, 08:52:59 AM
Why are you redefining DRIVE_SPEED_MIN and DRIVE_SPEED_MAX - they are defined in Webbotlib (with different values to yours - so you will be confusing Webbotlib enormously).
Since its just a serial interface to the Sabertooth there shouldn't be anything you can send it that will damage the board. So I think you must have shorted something.

Title: Re: Controlling motors [Urgent]
Post by: Mastermime on March 20, 2013, 09:37:29 PM
Quote
Why are you redefining DRIVE_SPEED_MIN and DRIVE_SPEED_MAX - they are defined in Webbotlib (with different values to yours - so you will be confusing Webbotlib enormously).
Ahh that was a mistake.  My new Sabertooth should be coming very soon thanks to Dimension Engineering's great support (highly recommended).

Does everything else look alright? 
Btw I'm sorry for my ignorance.  It's just that I'm more of an EE guy and my software skills are raw and rough.
Title: Re: Controlling motors [Urgent]
Post by: Mastermime on March 31, 2013, 07:47:33 PM
Ok I just finished rewiring my electrical to make it cleaner and less problematic.  I verified my wiring so that's correct.  Is it possible that my wiring was causing the simultaneous half speed full speed?  I did notice the error LED went off when this occurred, but I was able to run them both full speed by just setting them to full speed without any serial communication so this makes me think its my code.  I just don't see what would cause this