Skip to content

Instantly share code, notes, and snippets.

@natsud1
Created February 14, 2013 08:04
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save natsud1/e4d1dd511fd18eaba1cc to your computer and use it in GitHub Desktop.
Save natsud1/e4d1dd511fd18eaba1cc to your computer and use it in GitHub Desktop.
#include <SoftwareSerial.h>
#define BTtxPin 2
#define BTrxPin 3
SoftwareSerial BTSerial = SoftwareSerial(BTrxPin, BTtxPin);// set up a new serial port
void setup(){
pinMode ( BTrxPin, INPUT );
pinMode ( BTtxPin, OUTPUT );
Serial.begin(9600);
BTSerial.begin(9600);
}
void loop(){
readSPP();
}
void readSPP(){
#define DEBUG // Uncomment to print data for debugging
char input[30];
uint8_t i = 0;
while (1) { //i think this should maybe be something like while (BTSerial.available()) and then delete the if(incoming != 255){ but im not sure
uint8_t incoming = BTSerial.read();
if(incoming != 255){// there is some kind of info being sent continuously not sure what it is but this just filtered it out
input[i] = incoming;
if (input[i] == ';'){ // Keep reading until it reads a semicolon
break;
}
i++;
}
}
if(input[0] == 'J') { // Joystick
strtok(input, ","); // Ignore 'J'
float sppData1 = atof(strtok(NULL, ",")); // x-axis
float sppData2 = atof(strtok(NULL, ";")); // y-axis
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" X-axis ");
Serial.print(sppData1);
Serial.print(" Y-axis ");
Serial.println(sppData2);
#endif
}
else if(input[0] == 'M') { // IMU
strtok(input, ","); // Ignore 'I'
double sppData1 = atof(strtok(NULL, ",")); // Pitch
double sppData2 = atof(strtok(NULL, ";")); // Roll
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" Pitch ");
Serial.print(sppData1);
Serial.print(" Roll ");
Serial.println(sppData2);
#endif
}
else if(input[0] == 'S'){ // Stop
int steer = 512;
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" Steer = ");
Serial.println(steer);
#endif
}
else if(input[0] == 'P') {
strtok(input, ","); // Ignore 'P'
double Kp = atof(strtok(NULL, ";"));
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" P = ");
Serial.println(Kp);
#endif
}
else if(input[0] == 'I') {
strtok(input, ","); // Ignore 'I'
double Ki = atof(strtok(NULL, ";"));
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" I = ");
Serial.println(Ki);
#endif
}
else if(input[0] == 'D') {
strtok(input, ","); // Ignore 'D'
double Kd = atof(strtok(NULL, ";"));
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" D = ");
Serial.println(Kd);
#endif
}
else if(input[0] == 'T') { // Target Angle
strtok(input, ","); // Ignore 'T'
double targetAngle = atof(strtok(NULL, ";"));
#ifdef DEBUG
Serial.print("Flag ");
Serial.print(input[0]);
Serial.print(" T = ");
Serial.println(targetAngle);
#endif
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment