void ledBlink(int times) { for (int i = 0; i 0) { Serial.print("Command received from BT ==> "); Serial.println(device); command = device; device =""; //Reset the variable BT1.flush(); } } //------------------------------------------------------------------------ void manualCmd() { switch (command[0]) { case 'g': mode = FOLLOWING_LINE; break; case 's': motorStop(); //turn off both motors break; case 'f': motorForward(); break; case 'r': motorTurn(RIGHT, 30); motorStop(); break; case 'l': motorTurn(LEFT, 30); motorStop(); break; case 'b': motorBackward(); break; case 'p': Kp = command[2]; break; case 'i': Ki = command[2]; break; case 'd': Kd = command[2]; break; } } //------------------------------------------------------------------------ void sendBTdata (int data) // send data to BT { digitalWrite (ledPin, HIGH); BT1.print("Data from Arduino"); BT1.print(data); BT1.print(" xx"); BT1.println('\n'); digitalWrite (ledPin, LOW); } //-------------------------------------------------------- void calculatePID() { P = error; I = I + error; D = error-previousError; PIDvalue = (Kp*P) + (Ki*I) + (Kd*D); previousError = error; } //-------------------------------------------------------- void checkPIDvalues() { BT1.print("PID: "); BT1.print(Kp); BT1.print(" - "); BT1.print(Ki); BT1.print(" - "); BT1.println(Kd); Serial.print("PID: "); Serial.print(Kp); Serial.print(" - "); Serial.print(Ki); Serial.print(" - "); Serial.println(Kd); } //----------------------------------------------- void testLineFollowSensors() { int LFS0 = digitalRead(lineFollowSensor0); int LFS1 = digitalRead(lineFollowSensor1); int LFS2 = digitalRead(lineFollowSensor2); int LFS3 = digitalRead(lineFollowSensor3); int LFS4 = digitalRead(lineFollowSensor4); Serial.print ("LFS: L 0 1 2 3 4 R ==> "); Serial.print (LFS0); Serial.print (" "); Serial.print (LFS1); Serial.print (" "); Serial.print (LFS2); Serial.print (" "); Serial.print (LFS3); Serial.print (" "); Serial.print (LFS4); Serial.print (" ==> "); Serial.print (" P: "); Serial.print (P); Serial.print (" I: "); Serial.print (I); Serial.print (" D: "); Serial.print (D); Serial.print (" PID: "); Serial.println (PIDvalue); }