C Code (Robot)
Sorry about the spacing. When it transferred from the editor, the spacing doubled. For better formatting, go here
#include "hardware.h"
// Comment the next line out if not using input from the XBEE in which case the
// program will move the motors backwards and forwards
#define XBEE_INPUT
// Comment the next line out to disable output to the PC for debugging
// Final production robot will not be connected to the PC
#define DEBUG
// Comment the next line out to disable sending anything to the Sabertooth motors
// because you are just using the DEBUG output to the PC
#define USE_SABERTOOTH
// If robot doesnt hear from the handheld in this number of us then assume robot needs to brake
#ifdef XBEE_INPUT
#define COMMS_TIMEOUT 500000UL
#endif
#ifdef DEBUG
/* ----------------------- Terminal documentation ----------------------
The VT100 terminal is set up to display info as follows:-
Col
11111111112222222222333333333344444
012345678901234567890123456789012345678901234
Line
0: Packet: xxx
1: ------ Left ------- + ------ Right ------
2: Target Actual Accel | Target Actual Accel
3: xxxx xxxx xxxx | xxxx xxxx xxxx
4: ------------------- + -------------------
5: Fwd x x | x x
6: x x | x x
7: x x | x x
8: x x | x x
9: Stop x x | x x
10: x x | x x
11: x x | x x
12: x x | x x
13: Rev x x | x x
-------------------------------------------------------------------*/
// The center/Stop line number on any graph (see above)
#define GRAPH_LINE 9
// How many lines are used above and below the GRAPH_LINE
#define GRAPH_HEIGHT 4
// Constant divisor to turn a drive speed into a line number relative
// to GRAPH_LINE
#define GRAPH_DIVIDER (DRIVE_SPEED_MAX / GRAPH_HEIGHT)
#endif
#ifdef XBEE_INPUT
// Packet received from handheld controller, always a 3 digit number
#define PACKET_SIZE 3
char packet[PACKET_SIZE + 1]; // add one byte for the string terminator
int curByte;
TICK_COUNT lastContact; // time when robot last heard from the controller
#endif
enum {
GRAPH_TARGET = 0, // 0 = the target speed data
GRAPH_ACTUAL, // 1 = the actual speed data
GRAPH_PER_MOTOR // always the last entry as it gives the # of entries
};
// Define a class structure that will keep tabs on acceleration info for a given actuator
class MotorWithAcceleration {
public:
MotorWithAcceleration(Actuator* motor, DISPLAY_COLUMN col){
m_motor = motor;
m_target_column = col;
for(int i=0; i<GRAPH_PER_MOTOR; i++)
{
m_previous[i] = 0;
m_new[i] = GRAPH_LINE;
m_speed[i] = DRIVE_SPEED_BRAKE;
}
}
void updateGraphs(void);
Actuator* getMotor(void) const {
return m_motor;
}
DRIVE_SPEED getActualSpeed(void) const;
DRIVE_SPEED getTargetSpeed(void) const;
void setActualSpeed(DRIVE_SPEED target);
void setTargetSpeed(DRIVE_SPEED target);
void accelerate(TICK_COUNT timeNow);
private:
Actuator* m_motor;
DISPLAY_COLUMN m_target_column;
DRIVE_SPEED m_speed[GRAPH_PER_MOTOR];
DISPLAY_LINE m_previous[GRAPH_PER_MOTOR];
DISPLAY_LINE m_new[GRAPH_PER_MOTOR];
TICK_COUNT m_lastTime;
};
// Define the methods for MotorWithAcceleration
void MotorWithAcceleration::updateGraphs(void)
{
#ifdef DEBUG
for( int i = 0; i<GRAPH_PER_MOTOR; i++)
{
DISPLAY_COLUMN column = 7*i + m_target_column;
if (m_previous[i] != m_new[i])
{
// Value changed so clear old
if (m_previous[i] != 0)
{
debug.setXY(column,m_previous[i]);
debug.print(' ');
}
// Update old to the new value
m_previous[i] = m_new[i];
// and plot the new value
debug.setXY(column,m_previous[i]);
debug.print('#');
}
}
#endif
}
void MotorWithAcceleration::setTargetSpeed(DRIVE_SPEED target)
{
if (getTargetSpeed() != target)
{ // has changed
// Store the speed
m_speed[GRAPH_TARGET] = target;
#ifdef DEBUG
// Calculate new blob position
m_new[GRAPH_TARGET] = GRAPH_LINE - (target / GRAPH_DIVIDER);
// display the target speed
debug.setXY(m_target_column-1, GRAPH_LINE-GRAPH_HEIGHT-2); rprintfNum(10, 4, true, ' ', target);
#endif
}
}
void MotorWithAcceleration::setActualSpeed(DRIVE_SPEED actual)
{
if (getActualSpeed() != actual)
{ // has changed
// Store the speed
m_speed[GRAPH_ACTUAL] = actual;
#ifdef DEBUG
// Calculate new blob position
m_new[GRAPH_ACTUAL] = GRAPH_LINE - (actual / GRAPH_DIVIDER);
// display the actual speed
debug.setXY(m_target_column+6, GRAPH_LINE-GRAPH_HEIGHT-2); rprintfNum(10, 4, true, ' ', actual);
#endif
}
}
DRIVE_SPEED MotorWithAcceleration::getActualSpeed(void) const
{
return m_speed[GRAPH_ACTUAL];
}
DRIVE_SPEED MotorWithAcceleration::getTargetSpeed(void) const
{
return m_speed[GRAPH_TARGET];
}
// This is how often robot accelerate in uS ie 10000 is 10000us = 10ms = 0.01 seconds
// The larger this number then the slower the robot accelerates ie is less responsive
#define ACCELERATE_EVERY (400UL * 10UL)
void MotorWithAcceleration::accelerate(TICK_COUNT timeNow)
{
// Find elapsed time since last called
TICK_COUNT deltaTime=timeNow - m_lastTime;
DRIVE_SPEED speed = getActualSpeed();
DRIVE_SPEED target = getTargetSpeed();
while (deltaTime >= ACCELERATE_EVERY)
{
m_lastTime += ACCELERATE_EVERY;
deltaTime -= ACCELERATE_EVERY;
if (target > speed)
{
speed++;
}
else if (target < speed)
{
speed--;
}
}
#ifdef DEBUG
// display the new acceleration value
long accel = speed - getActualSpeed();
debug.setXY(m_target_column+13,GRAPH_LINE-GRAPH_HEIGHT-2); rprintfNum(10,4,true,' ',accel);
#endif
setActualSpeed(speed);
}
// Wrap each of the Sabertooth motors in a MotorWithAcceleration -
// names in brackets are names of the Sabertooth motors specified in Project Designer
MotorWithAcceleration leftMotor(&motor_left, 6);
MotorWithAcceleration rightMotor(&motor_right, 28);
/* ------------------------------------------------------------------
Initialize the hardware
Called from webbotlib
------------------------------------------------------------------- */
void appInitHardware(void) {
initHardware();
#ifdef XBEE_INPUT
delay_ms(10);
#endif
// Center the servo
myServo.setSpeed(0);
//rprintf("\nHardware initialized.\n\n");
}
/* ------------------------------------------------------------------
Initialise the display with stuff that doesn't change
------------------------------------------------------------------- */
void initDisplay(void){
#ifdef DEBUG
// Initialise the display according to comment at top of file
rprintfInit(&debug_uartSendByte);
debug.clear();
debug.setXY(0,0); debug.print_P(PSTR("Packet:"));
debug.setXY(0,1); debug.print_P(PSTR(" ------ Left ------- + ------ Right ------"));
debug.setXY(0,2); debug.print_P(PSTR(" Target Actual Accel | Target Actual Accel"));
debug.setXY(24,3); debug.print_P(PSTR("|"));
debug.setXY(0,4); debug.print_P(PSTR(" ------------------- + -------------------"));
debug.setXY(0,5); debug.print_P(PSTR("Fwd"));
debug.setXY(0,9); debug.print_P(PSTR("Stop"));
debug.setXY(0,13); debug.print_P(PSTR("Rev"));
for (DISPLAY_LINE line = 5; line <= 13; line++){
debug.setXY(24,line); debug.print('|');
}
#endif
}
/* ------------------------------------------------------------------
Initialize the software
Called from webbotlib
------------------------------------------------------------------- */
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
initDisplay(); // paint the debug display with constant info
return 0;
}
/* ------------------------------------------------------------------
Process all input from the hand held controller
Keep doing this until there is no input left so
that the receive buffer doesn't overflow
------------------------------------------------------------------- */
// Magic number values from the controller
#define TRIANGLE 256
#define START 257
#define L2 258
#define R2 259
#define MOTOR_L_MIN 0
#define MOTOR_L_MAX 127
#define MOTOR_R_MIN 128
#define MOTOR_R_MAX 255
void readInput(TICK_COUNT now){
#ifdef XBEE_INPUT
int tempbyte;
boolean anything = false;
while(tempbyte = Xbee.read(), tempbyte!=-1) {
anything = true;
char temp = (char)tempbyte;
if (temp >= '0' && temp <='9' )
{
packet[curByte++] = temp;
//rprintf(“Got a byte! %c\n”, temp);
if (curByte == PACKET_SIZE)
{
// Got a 3 digit number so turn into an integer
packet[curByte] = '\0';
uint16_t value = (uint16_t)atoi(packet);
#ifdef DEBUG
// Display the packet value
debug.setXY(8,0); rprintfNum(10, 3, FALSE, ' ', value);
#endif
switch(value){
case START:
Remoteswitch_relay.toggle();
break;
case TRIANGLE:
led1.toggle();
led2.toggle();
led3.toggle();
break;
case L2:
myServo.setSpeed(-80);
delay_ms(90);
myServo.setSpeed(0);
break;
case R2:
myServo.setSpeed(80);
delay_ms(90);
myServo.setSpeed(0);
break;
default:
if (value>=MOTOR_L_MIN && value <=MOTOR_L_MAX)
{
// Control the left motor
// Turn the Sabertooth specific value sent by the controller back into a webbotlib generic speed
DRIVE_SPEED speed = interpolate(value,0,127,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX);
// and set the target speed
leftMotor.setTargetSpeed(speed);
}
else if (value>=MOTOR_R_MIN && value <=MOTOR_R_MAX)
{
// Control the right motor
// Turn the Sabertooth specific value sent by the controller back into a webbotlib generic speed
DRIVE_SPEED speed = interpolate(value,128,255,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX);
// and set the target speed
rightMotor.setTargetSpeed(speed);
}
}
}
}
else
{
// not a digit (like 'A' for example) = start of packet so reset index
curByte = 0;
}
}
if (!anything)
{
// Haven't heard from the controller - test for timeout
TICK_COUNT elapsed = now - lastContact;
if (elapsed >= COMMS_TIMEOUT)
{
leftMotor.setTargetSpeed(DRIVE_SPEED_BRAKE);
rightMotor.setTargetSpeed(DRIVE_SPEED_BRAKE);
}
}
else
{
// Got something - so reset the timeout
lastContact = now;
}
#endif
}
/* ------------------------------------------------------------------
This is the main loop
Called from webbotlib
------------------------------------------------------------------- */
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
readInput(loopStart);
#ifndef XBEE_INPUT
// Not using XBee input so 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
}
DRIVE_SPEED speed = interpolate(now, 0, 5000, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
leftMotor.setTargetSpeed(speed);
rightMotor.setTargetSpeed(speed);
#endif
// Calculate new motor speeds
leftMotor.accelerate(loopStart);
rightMotor.accelerate(loopStart);
#ifdef USE_SABERTOOTH
// Send speed to Sabertooth
leftMotor.getMotor()->setSpeed(leftMotor.getActualSpeed());
rightMotor.getMotor()->setSpeed(rightMotor.getActualSpeed());
#endif
#ifdef DEBUG
// Update the debug display on the PC
leftMotor.updateGraphs();
rightMotor.updateGraphs();
#endif
return 0;
}