Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
Diff: main.cpp
- Revision:
- 3:d1197b5ea68a
- Parent:
- 2:0f80c8a1c236
- Child:
- 4:81b0de07841f
diff -r 0f80c8a1c236 -r d1197b5ea68a main.cpp --- a/main.cpp Sun Oct 12 13:33:19 2014 +0000 +++ b/main.cpp Sun Oct 12 23:27:43 2014 +0000 @@ -8,33 +8,29 @@ #include "tone.h" #include "ultrasonic.h" #include "bt_shell.h" -int Rmotor_input=0; -int Lmotor_input=0; int Current_Right_pulse=0; int Current_Left_pulse=0; -int Error_Right=0; -int Error_Left=0; int Linput=0; int Rinput=0; int Change_Right_pulse=0; int Change_Left_pulse=0; int Previous_Left_pulse=0; int Previos_Right_pulse=0; -int Last_Error_Right=0; -int Last_Error_Left=0; -int Rdistance_factor=0; -int Ldistance_factor=0; int time_int = 50; int time_factor=1000/time_int ; -int L_Kp=0; -int R_Kp=0; +int Error_Right=0; +int Error_Left=0; +float L_Kp=0.1; +float R_Kp=0.1; +int previous_Linput= 0 ; +int previous_Rinput= 0 ; RtosTimer *Motor_controller_Timer; void Motorcontroller(void const *args) { - Current_Right_pulse= right.getPulses(); - Current_Left_pulse=left.getPulses(); + Current_Right_pulse= right.getPulses()/5; + Current_Left_pulse=left.getPulses()*(-1)/5; Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse; Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse; @@ -42,25 +38,35 @@ Previous_Left_pulse=Current_Left_pulse; Previos_Right_pulse=Current_Right_pulse; - Error_Right=(Rmotor_input- (Change_Right_pulse*time_factor*Rdistance_factor) ); - Error_Left=(Lmotor_input- (Change_Left_pulse*time_factor*Ldistance_factor)); - Last_Error_Right=Error_Right; - Last_Error_Left=Error_Left; + Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) ); + Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor)); Linput=(Linput+Error_Left*L_Kp); Rinput=(Rinput+Error_Right*R_Kp); - if(Linput>100) Linput=100; else if (Linput<-100) Linput= -100; - else if (-21<Linput && Linput<21) + else if (-11<Linput && Linput<11) Linput= 0; if(Rinput>100) Rinput=100; else if(Rinput<-100) Rinput= -100; - else if (-21<Rinput && Rinput<21) + else if (-11<Rinput && Rinput<11) Rinput= 0; + if (Linput== previous_Linput && Rinput ==previous_Rinput) { + } else { + previous_Linput= Linput ; + previous_Rinput= Rinput ; + motor_control(-Rinput,-Linput); + } + /* + 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(); + */ } @@ -97,7 +103,7 @@ 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); + Motor_controller_Timer->start(time_int); while(1) { if(ldrread1()>0.6) { }