Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
Diff: main.cpp
- Revision:
- 4:81b0de07841f
- Parent:
- 3:d1197b5ea68a
- Child:
- 5:eb706d3e512c
diff -r d1197b5ea68a -r 81b0de07841f main.cpp --- a/main.cpp Sun Oct 12 23:27:43 2014 +0000 +++ b/main.cpp Tue Oct 14 17:55:37 2014 +0000 @@ -8,6 +8,7 @@ #include "tone.h" #include "ultrasonic.h" #include "bt_shell.h" +#include "bt_shell_f.h" int Current_Right_pulse=0; int Current_Left_pulse=0; int Linput=0; @@ -22,6 +23,8 @@ int Error_Left=0; float L_Kp=0.1; float R_Kp=0.1; +int Last_Error_Left=0; +int Last_Error_Right=0; int previous_Linput= 0 ; int previous_Rinput= 0 ; RtosTimer *Motor_controller_Timer; @@ -46,36 +49,39 @@ Linput=100; else if (Linput<-100) Linput= -100; - else if (-11<Linput && Linput<11) - Linput= 0; if(Rinput>100) Rinput=100; else if(Rinput<-100) Rinput= -100; - else if (-11<Rinput && Rinput<11) - Rinput= 0; - if (Linput== previous_Linput && Rinput ==previous_Rinput) { + if(Error_Right==0) { + Last_Error_Right++; } else { - previous_Linput= Linput ; - previous_Rinput= Rinput ; - motor_control(-Rinput,-Linput); + Last_Error_Right=0; + } + if(Error_Left==0) { + Last_Error_Left++; + } else { + Last_Error_Left=0; } - /* - bt.lock(); - bt.printf(">>D;%d;%d;%d;%d;%d;%d;%d;%d;\r\n", - Rmotor_speed,Lmotor_speed,Error_Right ,Error_Left,\ - Rinput,Linput,Change_Right_pulse,Change_Left_pulse); - bt.unlock(); - */ + if(Last_Error_Right==100) { + Rinput=0; + } + if(Last_Error_Left==100) { + Linput=0; + } + *motors.Left = Linput; // HARSH change to -Linput for black and Linput for blue + *motors.Right = -Rinput; //// HARSH change to Rinput for black and -Rinput for blue } - - char c; char buffer[10]; void bt_shell_thr(void const *args) { while(true) { - bt_shell_run(); + if(Selected_robot=='A') { + bt_shell_run(); + } else if(Selected_robot=='F') { + bt_shell_f_run(); + } Thread::wait(50); } } @@ -98,19 +104,28 @@ bt.gets(buffer, 5); } } - //imperial_march(); + bt_connected=true; + bt.gets(buffer, 2); + if(buffer[0]=='A') { + Selected_robot='A'; + imperial_march(); + + } else if(buffer[0]=='F') { + Selected_robot='F'; + Led_on(); + } + Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL); Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); - Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL); Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL); Motor_controller_Timer->start(time_int); while(1) { - if(ldrread1()>0.6) { - } - if(ldrread2()>0.6) { - Led_on(); - } else { - Led_off(); + if(Selected_robot=='A') { + if(ldrread2()>0.6) { + Led_on(); + } else { + Led_off(); + } } ultrasonic_run(); Thread::yield();