//===================================================================================================== // Bluetooth.ino //===================================================================================================== // // This code support a simple self-balancing robot based on the the Arduino 101 board // This code is focused on implementing communication with an Android App over a Bluetooth link. // // Date Author Notes // 04/24/2016 LCross Initial release // // BT Serial recieve communications function // BT Transmit commands: // Command format : // V

# - Response to V command to send PID term values. // C# - Text name of the PID controller selected for multiplier value change // B# - Battery voltage value. Also used as a Heart Beat Indicator. // M# - Text message // F# - Robot Firmware version // L/l# - confirm PID data logging on/off // E# - current pitch error compensation value // K# - current motor slack compensation value // D# - Datalog packet // P# - Robot pitch angle in degrees // S# - Robot speed in feet/sec // Function to send the current PID controller multiplier values to BT serial void btSendPIDvalues(int PIDindex) { Serial1.print('V'); for (int x=0; x < 3; x++) { Serial1.print(PIDmultVal[PIDindex][x], 4); if (x < 2) Serial1.print (" "); } Serial1.print('#'); } // Function to the name of the PID controller (as a text string) currently selected for multiplier value change void btSendPIDname(int PIDindex) { Serial1.print('C'); Serial1.print(PIDname[PIDindex]); Serial1.print('#'); } // Function to send battery voltage to BT serial void btBatteryVoltage(float voltage) { Serial1.print('B'); Serial1.print(voltage,1); Serial1.print('#'); } // Function to send Firmware version to BT serial void btFirmware(float fw_version) { Serial1.print('F'); Serial1.print(fw_version,2); Serial1.print("#"); } // Function to send Roll Error to BT serial void btRollError() { Serial1.print ("E"); Serial1.print (RollError, 2); Serial1.print ('#'); } // Function to send motor slack to BT serial void btMotorSlack() { Serial1.print ("K"); Serial1.print (motor.MotorSlack); Serial1.print ('#'); } // Function to send robot pitch to BT serial void btPitch(float pitch) { Serial1.print ("P"); Serial1.print (pitch,1); Serial1.print ('#'); } // Function to send robot pitch to BT serial void btSpeed(float robotSpeed) { Serial1.print ("S"); Serial1.print (robotSpeed,1); Serial1.print ('#'); } // BT Received commands: // Command format : // C# - Indicate that BT controller is connected // S# - Speed control. Range is 0 to 90 // N# - Steering control. Range is -90 to 90 // O# - Transition robot to standby mode (Off) // A# - Transition robot to active mode // V# - Request to send currently selected PID term values // P#/p# - Increment/Decrement P term // I#/i# - Increment/Decrement I term // D#/d# - Increment/Decrement D term // E# - Request to send roll error value // K# - Request to send motor slack value // R/r# - Increment/Decrement Roll Error value // T/t# - Increment/Decrement motor slack value // B# - Select next PID paramaters to change, send PID name back // L#/l# - Data logging on/(off or data log transfer complete) // k# - Datalog packet ack, send next void btComm () { char inChar = Serial1.read(); BTserial += inChar; if (inChar == '#'){ // watch for command termination character // Implement BT Commands switch (BTserial[0]) { case 'C' : // Bluetooth connected : C# Serial.println ("BT Controller Connected"); break; case 'S' : // Speed control : Snn# BTserial = BTserial.substring(1,BTserial.length()-1); BTspeed = map(BTserial.toInt(), 0, 90, -ROBOTMAXSPEED, ROBOTMAXSPEED); // Speed control from Android break; case 'N' : // Steering control : Nnn#, in deg/sec BTserial = BTserial.substring(1,BTserial.length()-1); BTsteer = map(BTserial.toInt(), -90, 90, -30, 30); break; case 'O' : // Put robot in Standby mode (off): O# BTstandby = HIGH; Serial.println ("BT request Robot Standby"); break; case 'A' : // Take robot out of standby (Active) : A# BTstandby = LOW; Serial.println ("BT request Robot Active"); break; case 'V' : // Return PID control values : V# btSendPIDvalues(BTPIDselect); break; case 'P' : // Increment P multiplier : P# PIDmultVal[BTPIDselect][0] += PID_P_INC; setPIDmulti(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'p' : // Decriment P multiplier : p# PIDmultVal[BTPIDselect][0] -= PID_P_INC; setPIDmulti(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'I' : // Increment I multiplier : I# PIDmultVal[BTPIDselect][1] += PID_I_INC; setPIDmulti(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'i' : // Decriment I multiplier : i# PIDmultVal[BTPIDselect][1] -= PID_I_INC; setPIDmulti(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'D' : // Increment D multiplier : D# PIDmultVal[BTPIDselect][2] += PID_D_INC; setPIDmulti(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'd' : // Decriment D multiplier : d# PIDmultVal[BTPIDselect][2] -= PID_D_INC; setPIDmulti(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'E' : // Return Roll Error value : E# btRollError(); break; case 'K' : // Return Motor Slack value : K# btMotorSlack(); break; case 'R' : // Increment roll error : R# RollError += ROLLERR_INC; btRollError(); break; case 'r' : // Decrement roll error : r# RollError -= ROLLERR_INC; btRollError(); break; case 'T' : // Increment motor slack: T# motor.MotorSlack += MOTORSLACK_INC; btMotorSlack(); break; case 't' : // Decrment motor slack: t# if (motor.MotorSlack >= 1) motor.MotorSlack -= MOTORSLACK_INC; btMotorSlack(); break; case 'B' : // Select next PID: B# BTPIDselect ++; if (BTPIDselect >= PIDcount) BTPIDselect = 0; // Wrap around when at end of list btSendPIDname(BTPIDselect); btSendPIDvalues(BTPIDselect); break; case 'L' : // Turn data logging on : L# Serial1.print("L#"); // send ack BTdatalog = HIGH; datalogindx = 0; DLstart = millis(); break; case 'l' : // Turn PID control loop data logging off : l# BTdatalog = LOW; Serial1.print("l#"); // send ack break; case 'k' : // Data log packet ack, send next packet: k# if (datalogindx < DATALOGSIZE) { Serial1.print("D"); for (int x=0; x < 7; x++) { Serial1.print(datalog[x][datalogindx],2); if (x < 6) Serial1.print(","); } Serial1.print("#"); datalogindx ++; } else Serial1.print("l#"); // Signal data log transfer complete break; default : Serial.println (BTserial); } BTserial = ""; // clear BT receive string } } // Functions to maintain and access PID multiplier values, changed over BT interface // Set PID multiplier values - first update array with new values, then call class member to update values // in class. If function is passed kp, ki and kd then update array with new values. // If only index is passed, use values already in the array. void setPIDmulti(int PIDindex, float kp, float ki, float kd) { PIDmultVal[PIDindex][0] = kp; PIDmultVal[PIDindex][1] = ki; PIDmultVal[PIDindex][2] = kd; setPIDmulti(PIDindex); } void setPIDmulti(int PIDindex) { switch (PIDindex) { case 0 : // Balance balancePID.kp = PIDmultVal[PIDindex][0]; balancePID.ki = PIDmultVal[PIDindex][1]; balancePID.kd = PIDmultVal[PIDindex][2]; break; case 1 : // Speed speedPID.kp = PIDmultVal[PIDindex][0]; speedPID.ki = PIDmultVal[PIDindex][1]; speedPID.kd = PIDmultVal[PIDindex][2]; break; } }