Diff: main.cpp

Revision:

4:81b0de07841f

Parent:

3:d1197b5ea68a

Child:

5:eb706d3e512c

--- 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();