Revision 4:81b0de07841f, committed 2014-10-14

Comitter:

Throwbot

Date:

Tue Oct 14 17:55:37 2014 +0000

Parent:

3:d1197b5ea68a

Child:

5:eb706d3e512c

Commit message:

Switching between amigobot and Finch bot can be done without resetting the robot

Changed in this revision

--- a/Drivers/Tone/tone.cpp	Sun Oct 12 23:27:43 2014 +0000
+++ b/Drivers/Tone/tone.cpp	Tue Oct 14 17:55:37 2014 +0000
@@ -26,7 +26,7 @@
 // Pulse the speaker to play a tone for a particular duration
 void imperial_march()
 {
-    for (int i=0; i<MAX_COUNT; i++) {
+    for (int i=0; i<36; i++) {
         toneM = melody1[i];
         beat = beats1[i];
         duration = beat * tempo; // Set up timing

--- a/Drivers/ultrasonic/ultrasonic.cpp	Sun Oct 12 23:27:43 2014 +0000
+++ b/Drivers/ultrasonic/ultrasonic.cpp	Tue Oct 14 17:55:37 2014 +0000
@@ -80,50 +80,4 @@
        UL_R=sensorvalue(uR);
        UL_F=sensorvalue(uF);
        stdio_mutex.unlock();
-       /* SRX=1;
-        wait_ms(5);
-        stdio_mutex.lock();
-        UL_rR=sensorvalue(urR);
-        stdio_mutex.unlock();
-        SRX=0;
-        wait_ms(5);
-
-        SRX=1;
-        wait_ms(5);
-        stdio_mutex.lock();
-        UL_R=sensorvalue(uR);
-        stdio_mutex.unlock();
-        SRX = 0;
-        wait_ms(5);
-
-        SRX=1;
-        wait_ms(5);
-        stdio_mutex.lock();
-        UL_F=sensorvalue(uF);
-        stdio_mutex.unlock();
-        SRX=0;
-        wait_ms(5);
-
-        SRX=1;
-        wait_ms(5);
-        stdio_mutex.lock();
-        UL_L=sensorvalue(uL);
-        stdio_mutex.unlock();
-        SRX=0;
-        wait_ms(5);
-
-        SRX=1;
-        wait_ms(5);
-        stdio_mutex.lock();
-        UL_rL=sensorvalue(urL);
-        stdio_mutex.unlock();
-        SRX=0;
-        wait_ms(5);
-
-        SRX=1;
-        wait_ms(5);
-        stdio_mutex.lock();
-        UL_B=sensorvalue(uB);
-        stdio_mutex.unlock();
-        SRX=0; */
 }

--- a/Robot.lib	Sun Oct 12 23:27:43 2014 +0000
+++ b/Robot.lib	Tue Oct 14 17:55:37 2014 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Abhishek/code/Robot_Original/#3e3314102e56
+http://developer.mbed.org/teams/Abhishek/code/Robot_Original/#0eeea5f05e28

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

--- a/shell/bt_shell.cpp	Sun Oct 12 23:27:43 2014 +0000
+++ b/shell/bt_shell.cpp	Tue Oct 14 17:55:37 2014 +0000
@@ -56,7 +56,7 @@
             linear_velocity_direction= buffer[2];
             rotational_velocity_value = buffer[3]<<8|buffer[4];
             rotational_velocity_direction= buffer[5];
-            
+
             if( linear_velocity_direction==0x01) {
                 linear_velocity_value=linear_velocity_value;
                 linear_velocity_value=linear_velocity_value;
@@ -79,11 +79,31 @@
         } else if(buffer[0] == 'P'  && buffer[1]== 'O') {
             software_interuupt=1;
             myreset=0;
-        } 
+        } else if(buffer[3] == 'E') {
+            bt.printf(">>1B");
+            bt.gets(buffer, 5);
+            if (buffer[3] =='O') {
+                bt.gets(buffer, 2);
+                bt_connected=true;
+                if(buffer[0]=='A') {
+                    Selected_robot='A';
+                    imperial_march();
+                } else if(buffer[0]=='F') {
+                    Selected_robot='F';
+                    Led_on();
+                }
+            }
+        } else if(buffer[3] == '?') {
+            bt.printf("eBot#2");
+        }
+        else if(buffer[3] == 'X') {         // Harsh please send <<1X when you are disconnecting 
+            bt_connected=false;             // from Amigobot it will realise the connection is stooped
+        }
         else {
             bt.rxBufferFlush();
         }
     }
     bt.unlock();
-    print_all();
+    if(bt_connected)
+        print_all();
 }
\ No newline at end of file

--- a/shell/bt_shell.h	Sun Oct 12 23:27:43 2014 +0000
+++ b/shell/bt_shell.h	Tue Oct 14 17:55:37 2014 +0000
@@ -2,6 +2,7 @@
 #define BT_shell_H
 #include "robot.h"
 #include "ultrasonic.h"
+#include "tone.h"
 
 extern "C" void mbed_reset();
 void bt_shell_run();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shell/bt_shell_f.cpp	Tue Oct 14 17:55:37 2014 +0000
@@ -0,0 +1,89 @@
+#include "bt_shell_f.h"
+//#include "keybindings.h"
+//the following functions are for the python interface
+
+//save a struct that indicates which data is to be returned
+interface_f iface_f;
+//mandatory tiny shell output function
+
+int char_received1=0;
+void bt_shell_f_run()
+{
+    char buffer[12];
+    bt.lock();
+    if(bt.readable()) {
+        bt.gets(buffer,3);
+        char_received1=buffer[0]-'0';
+        bt.unlock();
+        if(buffer[1]=='S') {
+            bt.lock();
+            stdio_mutex.lock();
+            bt.printf("%d;%d;%d;%d;%d;%d;",
+                      UL_rR,UL_R,UL_F,UL_L,UL_rL,UL_B);
+            stdio_mutex.unlock();
+            bt.unlock();
+        } else if(buffer[1]=='H') {
+            Lmotor_speed= 0;
+            Rmotor_speed = 0;
+            Led_off();
+        } else if(buffer[1]=='L') {
+            Led_on();
+        } else if(buffer[1]=='l') {
+
+            Led_off();
+        } else if(buffer[1]=='w') {
+            bt.gets(buffer,char_received1);
+            int Left_m_value = ((buffer[0]-'0')*100 + (buffer[1]-'0')*10 + (buffer[2]-'0')*1);
+            int Right_m_value= ((buffer[4]-'0')*100 + (buffer[5]-'0')*10 + (buffer[6]-'0')*1);
+            Lmotor_speed= (Left_m_value-200)*3.5;
+            Rmotor_speed = (Right_m_value-200)*3.5;
+        } else if(buffer[1]=='B') {
+            bt.gets(buffer,char_received1);
+            int buzzer_time = ((buffer[0]-'0')*1000 + (buffer[1]-'0')*100 + (buffer[2]-'0')*10 + (buffer[3]-'0')*1);
+            int frequency= ((buffer[5]-'0')*100 + (buffer[6]-'0')*10 + (buffer[7]-'0')*1);
+            pl_buzzer(frequency, buzzer_time);
+        } else if(buffer[1]=='D') {
+
+            bt.lock();
+            bt.printf("%lf;%lf",
+                      ldrread1(),ldrread2());
+            bt.unlock();
+        } else if(buffer[1]=='O') {
+            int obstacle_found=0;
+            if(UL_F<250) {
+                obstacle_found=1;
+            } else {
+                obstacle_found=0;
+            }
+            bt.lock();
+            bt.printf("%d",
+                      obstacle_found);
+            bt.unlock();
+        } else if(buffer[1]=='A') {
+            bt.lock();
+            stdio_mutex.lock();
+            bt.printf("%d;%d;%d",
+                      Encoder_x,Encoder_x,heading);
+            stdio_mutex.unlock();
+            bt.unlock();
+        } else if(buffer[1]=='<'  && buffer[0] == '<') {
+            bt.gets(buffer,3);
+            if(buffer[1] =='E')
+                bt.printf(">>1B");
+            else if (buffer[1] =='O') {
+                bt.gets(buffer, 2);
+                if(buffer[0]=='A') {
+                    Selected_robot='A';
+                    imperial_march();
+                } else if(buffer[0]=='F') {
+                    Selected_robot='F';
+                    Led_on();
+                }
+            }
+
+        } else {
+            bt.rxBufferFlush();
+        }
+    }
+    bt.unlock();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shell/bt_shell_f.h	Tue Oct 14 17:55:37 2014 +0000
@@ -0,0 +1,21 @@
+#ifndef BT_shell_f_H
+#define BT_shell_f_H
+#include "robot.h"
+#include "ultrasonic.h"
+#include "tone.h"
+
+void bt_shell_f_run();
+struct interface_f{
+    bool list[16];  //list saves which pieces of data must be returned
+        //0 - IMU_all
+        //1 - IMU_acc
+        //2 - IMU_gyr
+        //3 - IMU_rotation
+        //4 - IR_sensors
+        //5 - Optical_flow_sensors
+        //6 - Current_sensor
+        //7 - Voltage_sensor
+    int period; //this is the refresh rate at which the python wants data [ms]
+};
+
+#endif
\ No newline at end of file