And here is the ‘serial’ sketch in it’s current state;
byte ljoyver, ljoylat; //Received left joystick readings
byte rjoylat, rjoyver; //Received left joystick readings
byte digital; //Yet to be used, for determining if buttons have been pressed on controller
int vals[7]; //Stores received serial bytes
int checksum = 0;
byte southpaw;
int count = -1;
//int error = 13; //Error indication on LED pin 13
//int badpacket = 0; //Counts bad packets
//int stoppin = 54; //Connected to Sabertooth S2 on both controllers - not used on testbed.
//For digital inputs on controller
int digdata;
const byte dig1 = 1;
const byte dig2 = 2;
const byte dig3 = 4;
const byte dig4 = 8;
const byte dig5 = 16;
const byte dig6 = 32;
//Hull Sabertooth Motor Command bytes
const byte mixedforward = 8;
const byte mixedreverse = 9;
const byte mixedright = 10;
const byte mixedleft = 11;
//Turret Sabertooth Motor Command bytes
const byte rotleft = 0;
const byte rotright = 1;
const byte gunup = 4;
const byte gundown = 5;
//Commands returned from function hull() and turret().
byte command1, command2, command3, command4;
byte drive, steer, rotation, elevation;
void setup()
{
//digitalWrite(stoppin, HIGH); //If brought low shuts down Saberteeth
Serial.begin(38400);
Serial1.begin(38400); // start serial port controlling Sabertooth Controllers
delay(2000); // Required 2000 delay before sending 170(byte) to Sabertooth Controllors
Serial1.print(170, BYTE); // Bauding character to set Sabertooth Controllor's baud rate
// Digital pins
pinMode (40, OUTPUT);
pinMode (44, OUTPUT);
pinMode (46, OUTPUT);
pinMode (48, OUTPUT);
pinMode (50, OUTPUT);
pinMode (52, OUTPUT);
}
//Turret and Gun Control//
void turret(byte rjoylat, byte rjoyver)
{
//Gun elevation - Up commands//
if (rjoyver > 140)
{
command3 = gunup;
elevation = map(elevation, 141, 255, 1, 127);
}
//Gun elevation - Down commands//
else if (rjoyver < 115)
{
command3 = gundown;
elevation = (128 - (map(rjoyver, 1, 115, 1, 127)));
}
//Elevation Idle//
else
{
command3 = gunup;
elevation = 0;
}
//Turret - right rotation//
if (rjoylat > 140)
{
command4 = rotright;
rotation = map(rjoylat, 141, 255, 1, 127);
}
//Turret - left rotation//
else if (rjoylat < 115)
{
command4 = rotleft;
rotation = (128 - (map(rjoylat, 1, 115, 1, 127)));
}
//Rotation Idle//
else
{
command4 = rotleft;
rotation = 0;
}
sendelevation(command3, elevation);
sendrotation(command4, rotation);
}
//Hull control//
void hull(byte ljoyver, byte ljoylat)
{
//Forward Right Mixed Commands//
if (ljoyver > 140 && ljoylat < 115)
{
command1 = mixedforward;
command2 = mixedright;
drive = map(ljoyver, 140, 255, 1, 127);
steer = (115 - (map(ljoylat, 1, 115, 1, 127)));
}
//Forward Left Mixed Commands//
else if ((ljoyver > 140) && (ljoylat > 140))
{
command1 = mixedforward;
command2 = mixedleft;
drive = map(ljoyver, 141, 255, 1, 127); //Not mapped from 140 as it the minimum would be 141.
steer = (ljoylat - 140);
}
//Reverse Right Mixed Commands//
else if ((ljoyver < 115) && (ljoylat < 115))
{
command1 = mixedreverse;
command2 = mixedright;
drive = (128 - (map(ljoyver, 1, 115, 1, 127))); //0 = Stop 128 used to prevent this
steer = (115 - (map(ljoylat, 1, 115, 1, 127))); //Ditto
}
//Reverse Left Mixed Commands//
else if ((ljoyver < 115) && (ljoylat > 140))
{
command1 = mixedreverse;
command2 = mixedleft;
drive = (128 - (map(ljoyver, 1, 115, 1, 127)));
steer = map(ljoylat, 141, 255, 1, 127);
}
//Forward Mixed - Straight//
else if ((ljoyver > 140) && ((115 <= ljoylat) && (ljoylat <= 140)))
{
command1 = mixedforward;
command2 = mixedright;
drive = map(ljoyver, 141, 255, 1, 127);
steer = 0;
}
//Reverse Mixed - Straight//
else if ((ljoyver < 115) && ((115 <= ljoylat) && (ljoylat <= 140)))
{
command1 = mixedreverse;
command2 = mixedright;
drive = (128 - (map(ljoyver, 1, 115, 1, 127)));
steer = 0;
}
//Full Right Mixed//
else if (((115 <= ljoyver) && (ljoyver <= 140)) && (ljoylat < 115))
{
command1 = mixedforward;
command2 = mixedright;
drive = 0;
steer = (128 - (map(ljoylat, 1, 115, 1, 127)));
}
//Full Left Mixed//
else if (((115 <= ljoyver) && (ljoyver <= 140)) && (ljoylat > 140))
{
command1 = mixedreverse;
command2 = mixedright;
drive = 0;
steer = map(ljoylat, 141, 255, 1, 127);
}
//Both Motors Idle//
else //No conditions required -- all thats left
{
command1 = mixedreverse;
command2 = mixedright;
drive = 0;
steer = 0;
}
senddrive(command1, drive, command2, steer);//Should controll the hull sabertooth with two serial packets
}
void senddrive(byte command1, byte drive, byte command2, byte steer) //original
{
Serial1.print(130, BYTE); //Address
Serial1.print(command1, BYTE); //Direction
Serial1.print(drive, BYTE); //Rate
Serial1.print((130 + command1 + drive) & 0b01111111, BYTE); //Required checksum with mask
Serial1.print(130, BYTE); //Address
Serial1.print(command2, BYTE); //Direction
Serial1.print(steer, BYTE); //Rate
Serial1.print((130 + command2 + steer) & 0b01111111, BYTE); //Required checksum with mask
}
void sendelevation(byte command3, byte elevation) //original
{
Serial1.print(128, BYTE);
Serial1.print(command3, BYTE);
Serial1.print(elevation, BYTE);
Serial1.print((128 + command3 + elevation) & 0b01111111, BYTE); //Required checksum with mask
}
void sendrotation(byte command4, byte rotation) //original
{
Serial1.print(128, BYTE);
Serial1.print(command4, BYTE);
Serial1.print(rotation, BYTE);
Serial1.print((128 + command4 + rotation) & 0b01111111, BYTE); //Required checksum with mask
}
//Digital outs
//may change void to byte to accomodate use of digital or analog inputs ie; compass/gyro/acceler etc
//for target tracking or other applications.
void digitalout(byte digdata)
{
switch (digdata)
{
case dig1://RJ - Right joy button
digitalWrite(40, HIGH);
break;
case dig2://RB - Right B button
digitalWrite(44, HIGH);
break;
case dig3://RA - Right A button
digitalWrite(46, HIGH);
break;
case dig4://LB - Left B button
digitalWrite(48, HIGH);
break;
case dig5://LA - Left A button
digitalWrite(50, HIGH);
break;
case dig6://LJ - Left joy button
digitalWrite(52, HIGH);
break;
default://No buttons pressed this packet.
digitalWrite(40, LOW);
digitalWrite(44, LOW);
digitalWrite(46, LOW);
digitalWrite(48, LOW);
digitalWrite(50, LOW);
digitalWrite(52, LOW);
}
}
void loop()
{
while(Serial.available() > 0)
{
if(count == -1)
{ // new packet?
if(Serial.read() == 0xff)
{
count = 0;
checksum = 0;
}
}
else if(count == 0)
{
vals[count] = Serial.read();
if(vals[count] != 0xff)
{
checksum += vals[count];
count++;
}
}
else
{
vals[count] = Serial.read();
checksum += vals[count];
count++;
if(count == 7)
{// packet complete
if(checksum%256 != 255)
{// packet error!
}
else
{//Assign values from array
digital = vals[4];
southpaw = vals[5];
digdata = digital;
digitalout(digdata);
if((southpaw) == true)
{// SouthPaw controlls
ljoyver = vals[0];
ljoylat = vals[1];
rjoyver = vals[2];
rjoylat = vals[3];
}
else
{
rjoyver = vals[0];
rjoylat = vals[1];
ljoyver = vals[2];
ljoylat = vals[3];
}
}
count = -1;
Serial.flush(); //clear buffer
}
}
/*
Serial.print("rjoyver = ");
Serial.print(int(rjoyver));
Serial.print(", rjoylat = ");
Serial.print(int(rjoylat));
Serial.print("\n");
Serial.print("ljoyver = ");
Serial.print(int(ljoyver));
Serial.print(", ljoylat = ");
Serial.print(int(ljoylat));
Serial.print("\n");
Serial.print("digital = ");
Serial.print(int(digital));
Serial.print("\n");
*/
hull(ljoyver, ljoylat);
turret(rjoyver, rjoylat);
}
}
I have left out the setup information at the top of the sketch as it remains the same.
/me