Chaitanya Shinde / Mbed 2 deprecated SemiAutoCar

A four-wheeled vehicular prototype which imitates the structure of a modern car of rear wheeled motor transmission and controlling it using an android app via Bluetooth using FRDMK64F

Dependencies:   Servo mbed

main.cpp@0:0e05a4590d3b, 2017-03-30 (annotated)

Committer:
charlie316
Date:
Thu Mar 30 18:37:35 2017 +0000
Revision:
0:0e05a4590d3b
A four-wheeled vehicular prototype which imitates the structure of a modern car of rear wheeled motor transmission and controlling it using an android app via Bluetooth using FRDMK64F.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlie316 0:0e05a4590d3b 1 /* Code for manual controls of Smart Auto Car using Bluetooth */
charlie316 0:0e05a4590d3b 2 #include "mbed.h"
charlie316 0:0e05a4590d3b 3 //#include "Map.hpp"
charlie316 0:0e05a4590d3b 4
charlie316 0:0e05a4590d3b 5 #include "Servo.h"// The desired Servo sweep range is 60 degrees
charlie316 0:0e05a4590d3b 6 // But due to some mechanical Load errors it has been calibrated to 0 to 70 degrees
charlie316 0:0e05a4590d3b 7 // The front facing position varies from 27 degress to 38 degrees due to the servo motion slip.
charlie316 0:0e05a4590d3b 8
charlie316 0:0e05a4590d3b 9 // Variable declaration
charlie316 0:0e05a4590d3b 10 Servo myservo(D3);// Object to declare the servo functionalities
charlie316 0:0e05a4590d3b 11 int pos = 33;// Default avg front facing posiion of the Servo
charlie316 0:0e05a4590d3b 12 int i;
charlie316 0:0e05a4590d3b 13 int j;
charlie316 0:0e05a4590d3b 14 int i1;
charlie316 0:0e05a4590d3b 15 int j1;
charlie316 0:0e05a4590d3b 16
charlie316 0:0e05a4590d3b 17 int Flg=0; // Flag to enable motions (Forward, Backward, Stop state)
charlie316 0:0e05a4590d3b 18 int FlgSt = 0;// Unused Variable
charlie316 0:0e05a4590d3b 19 float Level = 0.30;// Variable to store the PWM Levels activated by the gear Button in the App.
charlie316 0:0e05a4590d3b 20 float pwm_Level;// Unused Variable
charlie316 0:0e05a4590d3b 21 // Function Initialisation
charlie316 0:0e05a4590d3b 22 void handleInput(int in);
charlie316 0:0e05a4590d3b 23 void drive_forward(float level);
charlie316 0:0e05a4590d3b 24 void drive_backward(float level);
charlie316 0:0e05a4590d3b 25 void Robot_Stop();
charlie316 0:0e05a4590d3b 26
charlie316 0:0e05a4590d3b 27 Serial pc(USBTX,USBRX);//Tx Rx for Serial Communication (UART)(PC to Processor)
charlie316 0:0e05a4590d3b 28 Serial BT(PTC15,PTC14);// Tx Rx for Bluetooth Communication ()
charlie316 0:0e05a4590d3b 29
charlie316 0:0e05a4590d3b 30 PwmOut ENApwm(D5); // Motor driver Enable A
charlie316 0:0e05a4590d3b 31 PwmOut ENBpwm(D6); // Motor driver Enable B
charlie316 0:0e05a4590d3b 32
charlie316 0:0e05a4590d3b 33 // Setting coresponding pins to output mode for Motor movements
charlie316 0:0e05a4590d3b 34 DigitalOut LeftMotor_P1(D8);
charlie316 0:0e05a4590d3b 35 DigitalOut LeftMotor_P2(D9);
charlie316 0:0e05a4590d3b 36 DigitalOut RightMotor_P1(D11);
charlie316 0:0e05a4590d3b 37 DigitalOut RightMotor_P2(D10);
charlie316 0:0e05a4590d3b 38
charlie316 0:0e05a4590d3b 39
charlie316 0:0e05a4590d3b 40 // The main Function
charlie316 0:0e05a4590d3b 41 int main() {
charlie316 0:0e05a4590d3b 42 pc.baud(9600);// UART communication in 9600 bits per seconds
charlie316 0:0e05a4590d3b 43 BT.baud(9600);// Bluetooth communication in 9600 bits per seconds
charlie316 0:0e05a4590d3b 44 // Printing some execution statements to ensure the controller is communicating properly.
charlie316 0:0e05a4590d3b 45 pc.printf(" Hello User!\r\n");
charlie316 0:0e05a4590d3b 46 printf(" Are You Ready \r\n");
charlie316 0:0e05a4590d3b 47 // At the start set servo to its default front facing position
charlie316 0:0e05a4590d3b 48 if(pos>33){
charlie316 0:0e05a4590d3b 49 myservo = (27/100.0);
charlie316 0:0e05a4590d3b 50 }
charlie316 0:0e05a4590d3b 51 else{
charlie316 0:0e05a4590d3b 52 myservo = (38/100.0);
charlie316 0:0e05a4590d3b 53 }
charlie316 0:0e05a4590d3b 54
charlie316 0:0e05a4590d3b 55
charlie316 0:0e05a4590d3b 56 while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function
charlie316 0:0e05a4590d3b 57 if (BT.readable()){ // Read incoming Bluetooth Data
charlie316 0:0e05a4590d3b 58 int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
charlie316 0:0e05a4590d3b 59 //int i = atoi((int)BT_Val); // Convert it to Interger.
charlie316 0:0e05a4590d3b 60 //BT_Val -='0';
charlie316 0:0e05a4590d3b 61 pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
charlie316 0:0e05a4590d3b 62 // Bluetooth data below decimal 25 contains certain values for the button operations
charlie316 0:0e05a4590d3b 63 // Bluetooth data above 25 triggers the PWM pins of EnA and EnB for motor speed control
charlie316 0:0e05a4590d3b 64
charlie316 0:0e05a4590d3b 65 if (BT_Val >= 25 && BT_Val <=100){
charlie316 0:0e05a4590d3b 66 Level = ((float)BT_Val/100);
charlie316 0:0e05a4590d3b 67 pc.printf("%f \r\n",Level);
charlie316 0:0e05a4590d3b 68 //float pwm_Level = Level;
charlie316 0:0e05a4590d3b 69 }
charlie316 0:0e05a4590d3b 70 else if (BT_Val < 25){
charlie316 0:0e05a4590d3b 71 handleInput(BT_Val);
charlie316 0:0e05a4590d3b 72 }
charlie316 0:0e05a4590d3b 73 if (Flg == 1){
charlie316 0:0e05a4590d3b 74 drive_forward((float)Level);
charlie316 0:0e05a4590d3b 75 }
charlie316 0:0e05a4590d3b 76 else if (Flg == 2){
charlie316 0:0e05a4590d3b 77 drive_backward((float)Level);
charlie316 0:0e05a4590d3b 78 }
charlie316 0:0e05a4590d3b 79 else if (Flg == 0){
charlie316 0:0e05a4590d3b 80 Robot_Stop();
charlie316 0:0e05a4590d3b 81 }
charlie316 0:0e05a4590d3b 82
charlie316 0:0e05a4590d3b 83 //if BT_Val == 'B'
charlie316 0:0e05a4590d3b 84 }
charlie316 0:0e05a4590d3b 85 }
charlie316 0:0e05a4590d3b 86 }
charlie316 0:0e05a4590d3b 87
charlie316 0:0e05a4590d3b 88 // Function which calls specific execution for corresponding incomming Serial inputs from Bluetooth
charlie316 0:0e05a4590d3b 89
charlie316 0:0e05a4590d3b 90 void handleInput(int in) {
charlie316 0:0e05a4590d3b 91 switch(in) {
charlie316 0:0e05a4590d3b 92 //Motor Rotation Controls
charlie316 0:0e05a4590d3b 93 case 6 :// Forward Motion
charlie316 0:0e05a4590d3b 94 Flg = 1;
charlie316 0:0e05a4590d3b 95 break;
charlie316 0:0e05a4590d3b 96 case 7 :
charlie316 0:0e05a4590d3b 97 Flg = 2;// Backward Motion
charlie316 0:0e05a4590d3b 98 break;
charlie316 0:0e05a4590d3b 99 case 0 : // Robot Stop
charlie316 0:0e05a4590d3b 100 Flg = 0;
charlie316 0:0e05a4590d3b 101 break;
charlie316 0:0e05a4590d3b 102 //Servo Rotation Controls
charlie316 0:0e05a4590d3b 103 case 1:// Turn 30 degree Left and back to front facing
charlie316 0:0e05a4590d3b 104 if (pos >0){
charlie316 0:0e05a4590d3b 105 myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 106 for( j1=pos; j1>=0; j1--) {
charlie316 0:0e05a4590d3b 107 myservo = (j1/100.0);
charlie316 0:0e05a4590d3b 108 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 109 wait(0.01);
charlie316 0:0e05a4590d3b 110 }
charlie316 0:0e05a4590d3b 111 pos=j1;
charlie316 0:0e05a4590d3b 112 for(i1=pos; i1<=38; i1++) {
charlie316 0:0e05a4590d3b 113 myservo = (i1/100.0);
charlie316 0:0e05a4590d3b 114 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 115 wait(0.01);
charlie316 0:0e05a4590d3b 116 }
charlie316 0:0e05a4590d3b 117 pos=i1;
charlie316 0:0e05a4590d3b 118 }
charlie316 0:0e05a4590d3b 119 else{
charlie316 0:0e05a4590d3b 120 myservo = (0/100.0);
charlie316 0:0e05a4590d3b 121 pos = 0;
charlie316 0:0e05a4590d3b 122 for(i1=pos; i1<=38; i1++) {
charlie316 0:0e05a4590d3b 123 myservo = (i1/100.0);
charlie316 0:0e05a4590d3b 124 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 125 wait(0.01);
charlie316 0:0e05a4590d3b 126 }
charlie316 0:0e05a4590d3b 127 pos=i1;
charlie316 0:0e05a4590d3b 128 }
charlie316 0:0e05a4590d3b 129 break;
charlie316 0:0e05a4590d3b 130
charlie316 0:0e05a4590d3b 131 case 2: //Turn 30 degree Left
charlie316 0:0e05a4590d3b 132 if (pos >0){
charlie316 0:0e05a4590d3b 133 //myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 134 for( j1=pos; j1>=0; j1--) {
charlie316 0:0e05a4590d3b 135 myservo = (j1/100.0);
charlie316 0:0e05a4590d3b 136 // printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 137 wait(0.01);
charlie316 0:0e05a4590d3b 138 }
charlie316 0:0e05a4590d3b 139 pos=j1;
charlie316 0:0e05a4590d3b 140 }
charlie316 0:0e05a4590d3b 141 else{
charlie316 0:0e05a4590d3b 142 myservo = (0/100.0);
charlie316 0:0e05a4590d3b 143 }
charlie316 0:0e05a4590d3b 144 break;
charlie316 0:0e05a4590d3b 145
charlie316 0:0e05a4590d3b 146 case 3: // Sweep Servo to forward position irrespective of its current position
charlie316 0:0e05a4590d3b 147 if(pos >40){
charlie316 0:0e05a4590d3b 148 //myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 149 for( j=pos; j>=27; j--) {
charlie316 0:0e05a4590d3b 150 myservo = (j/100.0);
charlie316 0:0e05a4590d3b 151 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 152 wait(0.01);
charlie316 0:0e05a4590d3b 153 }
charlie316 0:0e05a4590d3b 154 pos=j;
charlie316 0:0e05a4590d3b 155
charlie316 0:0e05a4590d3b 156 }
charlie316 0:0e05a4590d3b 157 else if (pos < 25){
charlie316 0:0e05a4590d3b 158 //myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 159 for(i1=pos; i1<=38; i1++) {
charlie316 0:0e05a4590d3b 160 myservo = (i1/100.0);
charlie316 0:0e05a4590d3b 161 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 162 wait(0.01);
charlie316 0:0e05a4590d3b 163 }
charlie316 0:0e05a4590d3b 164 pos=i1;
charlie316 0:0e05a4590d3b 165
charlie316 0:0e05a4590d3b 166 }
charlie316 0:0e05a4590d3b 167 else{
charlie316 0:0e05a4590d3b 168 myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 169 }
charlie316 0:0e05a4590d3b 170 break;
charlie316 0:0e05a4590d3b 171
charlie316 0:0e05a4590d3b 172 case 4://Turn 30 degree Right
charlie316 0:0e05a4590d3b 173 if (pos <70){
charlie316 0:0e05a4590d3b 174 // myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 175 for(i=pos; i<=70; i++) {
charlie316 0:0e05a4590d3b 176 myservo = (i/100.0);
charlie316 0:0e05a4590d3b 177 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 178 wait(0.01);
charlie316 0:0e05a4590d3b 179 }
charlie316 0:0e05a4590d3b 180 pos=i;
charlie316 0:0e05a4590d3b 181 }
charlie316 0:0e05a4590d3b 182 else{
charlie316 0:0e05a4590d3b 183 myservo = (70/100.0);
charlie316 0:0e05a4590d3b 184 }
charlie316 0:0e05a4590d3b 185 break;
charlie316 0:0e05a4590d3b 186
charlie316 0:0e05a4590d3b 187 case 5:// Turn 30 degree Right and back to front facing
charlie316 0:0e05a4590d3b 188 if (pos <70){
charlie316 0:0e05a4590d3b 189 //myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 190 for(i=pos; i<=70; i++) {
charlie316 0:0e05a4590d3b 191 myservo = (i/100.0);
charlie316 0:0e05a4590d3b 192 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 193 wait(0.01);
charlie316 0:0e05a4590d3b 194 }
charlie316 0:0e05a4590d3b 195 pos=i;
charlie316 0:0e05a4590d3b 196 for( j=pos; j>=27; j--) {
charlie316 0:0e05a4590d3b 197 myservo = (j/100.0);
charlie316 0:0e05a4590d3b 198 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 199 wait(0.01);
charlie316 0:0e05a4590d3b 200 }
charlie316 0:0e05a4590d3b 201 pos=j;
charlie316 0:0e05a4590d3b 202 }
charlie316 0:0e05a4590d3b 203 else{
charlie316 0:0e05a4590d3b 204 myservo = (70/100.0);
charlie316 0:0e05a4590d3b 205 pos = 70;
charlie316 0:0e05a4590d3b 206 for( j=pos; j>=27; j--) {
charlie316 0:0e05a4590d3b 207 myservo = (j/100.0);
charlie316 0:0e05a4590d3b 208 //printf("%f\r\n",myservo.read());
charlie316 0:0e05a4590d3b 209 wait(0.01);
charlie316 0:0e05a4590d3b 210 }
charlie316 0:0e05a4590d3b 211 pos=j;
charlie316 0:0e05a4590d3b 212 }
charlie316 0:0e05a4590d3b 213 //break;
charlie316 0:0e05a4590d3b 214 break;
charlie316 0:0e05a4590d3b 215
charlie316 0:0e05a4590d3b 216 case 8:// Turn Servo Left by 5 degrees irrespective of its current position
charlie316 0:0e05a4590d3b 217 pos = pos-5;
charlie316 0:0e05a4590d3b 218 myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 219 if (pos < 0){
charlie316 0:0e05a4590d3b 220 pos = 0;
charlie316 0:0e05a4590d3b 221 }
charlie316 0:0e05a4590d3b 222 break;
charlie316 0:0e05a4590d3b 223
charlie316 0:0e05a4590d3b 224 case 9: // Turn Servo right by 5 degrees irrespective of its current position
charlie316 0:0e05a4590d3b 225 pos = pos+5;
charlie316 0:0e05a4590d3b 226 myservo = (pos/100.0);
charlie316 0:0e05a4590d3b 227 if (pos > 70){
charlie316 0:0e05a4590d3b 228 pos = 70;
charlie316 0:0e05a4590d3b 229 }
charlie316 0:0e05a4590d3b 230 break;
charlie316 0:0e05a4590d3b 231
charlie316 0:0e05a4590d3b 232 default:
charlie316 0:0e05a4590d3b 233 pc.printf("I don't know: \"%d\"\n", in);
charlie316 0:0e05a4590d3b 234 break;
charlie316 0:0e05a4590d3b 235 }
charlie316 0:0e05a4590d3b 236 }
charlie316 0:0e05a4590d3b 237
charlie316 0:0e05a4590d3b 238 // Function to move rear wheel DC motors forward
charlie316 0:0e05a4590d3b 239
charlie316 0:0e05a4590d3b 240 void drive_forward(float level){
charlie316 0:0e05a4590d3b 241 ENApwm.write(level);
charlie316 0:0e05a4590d3b 242 ENBpwm.write(level);
charlie316 0:0e05a4590d3b 243 LeftMotor_P1=1;
charlie316 0:0e05a4590d3b 244 LeftMotor_P2=0;
charlie316 0:0e05a4590d3b 245 RightMotor_P1=1;
charlie316 0:0e05a4590d3b 246 RightMotor_P2=0;
charlie316 0:0e05a4590d3b 247 }
charlie316 0:0e05a4590d3b 248
charlie316 0:0e05a4590d3b 249 // Function to move rear wheel DC motors backward
charlie316 0:0e05a4590d3b 250 void drive_backward(float level){
charlie316 0:0e05a4590d3b 251 ENApwm.write(level);
charlie316 0:0e05a4590d3b 252 ENBpwm.write(level);
charlie316 0:0e05a4590d3b 253 LeftMotor_P1=0;
charlie316 0:0e05a4590d3b 254 LeftMotor_P2=1;
charlie316 0:0e05a4590d3b 255 RightMotor_P1=0;
charlie316 0:0e05a4590d3b 256 RightMotor_P2=1;
charlie316 0:0e05a4590d3b 257 }
charlie316 0:0e05a4590d3b 258
charlie316 0:0e05a4590d3b 259
charlie316 0:0e05a4590d3b 260 // Function to stop rear wheel motion
charlie316 0:0e05a4590d3b 261 void Robot_Stop() {
charlie316 0:0e05a4590d3b 262 //ENApwm.write(level);
charlie316 0:0e05a4590d3b 263 //ENBpwm.write(level);
charlie316 0:0e05a4590d3b 264 LeftMotor_P1=1;
charlie316 0:0e05a4590d3b 265 LeftMotor_P2=1;
charlie316 0:0e05a4590d3b 266 RightMotor_P1=1;
charlie316 0:0e05a4590d3b 267 RightMotor_P2=1;
charlie316 0:0e05a4590d3b 268 }