Diff: main.cpp

Revision:

3:d1197b5ea68a

Parent:

2:0f80c8a1c236

Child:

4:81b0de07841f

--- 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) {
         }