/* Connects to a Bluetooth module and parses data sent from a device. Works with Arduino Uno and also tested to work with a Digispark (must remove all Serial.print) Expects string data to come as follows: sX=[xVal],Y=[yVal]e - used for sending X/Y data. Example: sX=75,Y=55e. Handles +/- sC=[cVal]e - used for sending C (command button) data. Example: sC=1e */ #include #define RxPin 3 #define TxPin 4 #define SOP 's' #define EOP 'e' #define leftMtrDirPin 2 #define leftMtrSpdPin 0 #define rightMtrDirPin 5 #define rightMtrSpdPin 1 //int led1 = 8; //int led2 = 9; char inData[20]; byte index; int xVal=0, xAdj=0, yVal=0, yAdj=0, cVal=0; int leftSpeed=0, rightSpeed=0; int leftDir = HIGH, rightDir = HIGH; boolean debug = false; bool started = false; bool ended = false; SoftwareSerial blueToothSerial(RxPin, TxPin); // RX, TX void setup() { Serial.begin(9600); blueToothSerial.begin(9600); // Other stuff... pinMode(RxPin, INPUT); pinMode(TxPin, OUTPUT); pinMode(leftMtrSpdPin, OUTPUT); pinMode(leftMtrDirPin, OUTPUT); pinMode(rightMtrDirPin, OUTPUT); pinMode(rightMtrSpdPin, OUTPUT); // pinMode(led1, OUTPUT); // pinMode(led2, OUTPUT); } void loop() { xVal=0; yVal=0; cVal=0; // Read all serial data available, as fast as possible //while(Serial.available() > 0) while(blueToothSerial.available() > 0) { //char inChar = Serial.read(); char inChar = blueToothSerial.read(); if(inChar == SOP) { index = 0; inData[index] = '\0'; started = true; ended = false; } else if(inChar == EOP) { ended = true; break; } else { if(index < 19) { inData[index] = inChar; index++; inData[index] = '\0'; } } } // We are here either because all pending serial // data has been read OR because an end of // packet marker arrived. Which is it? if(started && ended) { // The end of packet marker arrived. Process the packet char *name = strtok(inData, "="); while(name) { char *valToken = strtok(NULL, ","); if(valToken) { int val = atoi(valToken); if(strcmp(name, "X") == 0) { xVal = val; //digitalWrite(led1, HIGH); } else if(strcmp(name, "Y") == 0) { yVal = val; //digitalWrite(led2, HIGH); } else if(strcmp(name, "C") == 0) { cVal = val; //digitalWrite(led3, HIGH); } // More else if's go here if more features are added to BT Bot Control - Android App. } name = strtok(NULL, "="); } //digitalWrite(led1, LOW); //digitalWrite(led2, LOW); //testing. //** Note - debugging to the Serial Monitor will cause Serial.available to crap out and make it seem // like the device has stopped outputting BT data. The reason is that the Serial buffer overflowed due to //printing debuggin statements. Turn debugging on only to verify you are getting data and test a few values //output values to Serial Monitor if (debug) { //Serial.print("X="); //Serial.print(xVal); //Serial.print(" Y="); //Serial.print(yVal); //Serial.print(" C="); //Serial.println(cVal); } if ((xVal !=0) && (yVal != 0)) { driveMotors(xVal, yVal); }else{ //turn motors off analogWrite(leftMtrSpdPin, 0); analogWrite(rightMtrSpdPin, 0); } //you can also process the commands here as well. // Reset for the next packet started = false; ended = false; index = 0; inData[index] = '\0'; } xVal=0, yVal=0, cVal=0; } void driveMotors(int xVal, int yVal) { //both motors forward full speed //digitalWrite(MotorADir, HIGH); //forward //digitalWrite(MotorBDir, LOW); //analogWrite(MotorASpeed, 100); //100 will be the lowest speed to start at. //analogWrite(MotorBSpeed, 100); float xPct=1.0; xAdj = map(xVal, 0, 100, 50, 255); //start motor speed of 50 is as low as these motors will work. Your motors may be different yAdj = map(yVal, 0, 100, 50, 255); //start motor speed of 50 is as low as these motors will work. Your motors may be different //xPct = map(abs(xAdj), 0, 255, 1, 0); xPct = mapf((float)abs(xAdj), (float)0, (float)255, (float)1, (float)0); if (yVal > 0) { //robot driving fwd - now determine left and right. digitalWrite(leftMtrDirPin, HIGH); digitalWrite(rightMtrDirPin, LOW); if (xVal < 0) //going left { analogWrite(leftMtrSpdPin, yAdj * xPct); //drive the motor analogWrite(rightMtrSpdPin, yAdj); //drive the motor //if(debug) {Serial.print("FwLF: "); printDouble(xPct,10);} }else{ //going right analogWrite(leftMtrSpdPin, yAdj); //drive the motor analogWrite(rightMtrSpdPin, yAdj * xPct); //drive the motor //if(debug) {Serial.print("FwRT: "); printDouble(xPct,10);} } } else //y val is < 0 - reversing { //robot driving backwards - now determine left and right. digitalWrite(leftMtrDirPin, LOW); digitalWrite(rightMtrDirPin, HIGH); if (xVal < 0) //backing up robot while turning right { analogWrite(leftMtrSpdPin, yAdj * xPct); //drive the motor analogWrite(rightMtrSpdPin, yAdj); //drive the motor //if(debug) {Serial.print("RevLF: "); printDouble(xPct,10);} }else{ //going right analogWrite(leftMtrSpdPin, yAdj); //drive the motor analogWrite(rightMtrSpdPin, yAdj * xPct); //drive the motor //if(debug) {Serial.print("RevRT: "); printDouble(xPct,10);} } } //analogWrite(leftMtrSpdPin, 0); //drive the motor //analogWrite(rightMtrSpdPin, 0); //drive the motor //slight delay delay(50); leftSpeed = 0, rightSpeed=0; } void printDouble( double val, unsigned int precision){ // prints val with number of decimal places determine by precision // NOTE: precision is 1 followed by the number of zeros for the desired number of decimial places // example: printDouble( 3.1415, 100); // prints 3.14 (two decimal places) Serial.print (int(val)); //prints the int part Serial.print("."); // print the decimal point unsigned int frac; if(val >= 0) frac = (val - int(val)) * precision; else frac = (int(val)- val ) * precision; Serial.println(frac,DEC); } //use to map values to float. float mapf (float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; }