Revision 2:0f80c8a1c236, committed 2014-10-12

Comitter:

Throwbot

Date:

Sun Oct 12 13:33:19 2014 +0000

Parent:

1:1da89c13dfa1

Child:

3:d1197b5ea68a

Commit message:

Testing Ultrasonic

Changed in this revision

--- a/Drivers/ultrasonic/ultrasonic.cpp	Sat Oct 11 03:53:27 2014 +0000
+++ b/Drivers/ultrasonic/ultrasonic.cpp	Sun Oct 12 13:33:19 2014 +0000
@@ -1,8 +1,14 @@
 #include "ultrasonic.h"
 #include "math.h"
-int arraysize =10;
-int rangevalue[10];
+int arraysize =20;
+int rangevalue[20];
 int modE;
+int UL_rR = 0;
+int UL_R = 0;
+int UL_F = 0;
+int UL_L = 0;
+int UL_rL = 0;
+int UL_B = 0;
 void isort(int *a, int n)
 {
 // *a is an array pointer function
@@ -59,4 +65,53 @@
     //modE = mode(rangevalue,arraysize);
     modE=rangevalue[arraysize/2];
     return modE;
-}
\ No newline at end of file
+}
+void ultrasonic_run()
+{
+        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/Drivers/ultrasonic/ultrasonic.h	Sat Oct 11 03:53:27 2014 +0000
+++ b/Drivers/ultrasonic/ultrasonic.h	Sun Oct 12 13:33:19 2014 +0000
@@ -2,8 +2,14 @@
 #define ULTRASONIC_H
 #include "mbed.h" 
 #include "robot.h"
-
+extern int UL_rR;
+extern int UL_R;
+extern int UL_F;
+extern int UL_L;
+extern int UL_rL;
+extern int UL_B;
 void isort(int *a, int n);
 int mode(int *x,int n);
 int sensorvalue(AnalogIn pin);
+void ultrasonic_run();
 #endif
\ No newline at end of file

--- a/Robot.lib	Sat Oct 11 03:53:27 2014 +0000
+++ b/Robot.lib	Sun Oct 12 13:33:19 2014 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Abhishek/code/Robot_Original/#282467cbebb3
+http://developer.mbed.org/teams/Abhishek/code/Robot_Original/#37a19a9e5f2c

--- a/main.cpp	Sat Oct 11 03:53:27 2014 +0000
+++ b/main.cpp	Sun Oct 12 13:33:19 2014 +0000
@@ -7,33 +7,76 @@
 #include "I2Cdev.h"
 #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;
+RtosTimer *Motor_controller_Timer;
+
+void Motorcontroller(void const *args)
+{
+
+    Current_Right_pulse= right.getPulses();
+    Current_Left_pulse=left.getPulses();
+
+    Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse;
+    Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse;
+
+    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;
+    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)
+        Linput= 0;
+    if(Rinput>100)
+        Rinput=100;
+    else if(Rinput<-100)
+        Rinput= -100;
+    else if (-21<Rinput && Rinput<21)
+        Rinput= 0;
+}
+
+
 char c;
 char buffer[10];
-int bit_size=20;
-int thetha1=300;
-int thetha=0;
-int stall=0;
-int bump=1;
-int UL_rR = 0;
-int UL_R = 0;
-int UL_F = 0;
-int UL_L = 0;
-int UL_rL = 0;
-int UL_B = 0;
-int  dx1=100;
-int dy1=200;
-
-int linear_velocity_value ;
-int linear_velocity_direction;
-int rotational_velocity_value ;
-int rotational_velocity_direction;
-int Lspeed=1;
-int Rspeed=1;
+void bt_shell_thr(void const *args)
+{
+    while(true) {
+        bt_shell_run();
+        Thread::wait(50);
+    }
+}
 
 int main()
 {
     initRobot();
-    //motor_control((100),(-100));
     while(buffer[3] != 'O') {
         while(buffer[3] != 'E') {
             if(bt.readable()) {
@@ -41,7 +84,7 @@
                 if(buffer[3] == 'E') {
                     bt.printf(">>1B");
                 } else if(buffer[3] == '?') {
-                    bt.printf("K");
+                    bt.printf("eBot#2");
                 }
             }
         }
@@ -49,106 +92,13 @@
             bt.gets(buffer, 5);
         }
     }
-     
     //imperial_march();
     Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
     Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
-    buzzer=0;
-    //motor_control(lMotor*m_speed,rMotor*m_speed*0.8);
-    //motor_control(lMotor*m_speed,0);
+    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) {
-        //bt.lock();
-        //bt.printf("LDR1_%lf;LDR2_%lf;Pulses is: %i  Revolutions is: %i Pulses is: %i  ultrasonic: , \r\n",ldrread1(),ldrread2(), left.getPulses(), left.getRevolutions(),right.getPulses());//, uL.read()*5*102.54*2.54);
-        //bt.printf("s;LDR1_%lf;LDR2_%lf;Pulses is:  %i ; %i:s\n\r",ldrread1(),ldrread2(),wheel.getPulses(), wheel.getRevolutions());
-        // motor_control(lMotor*m_speed,rMotor*m_speed);
-        //bt.printf("a/g:\t %d:\t %d:\t %d:\t %d:\t %d:\t %d:\t\r\n",ax ,ay,az,gx,gy,gz);
-        //bt.unlock();
-        if(bt.readable()) {
-            bt.gets(buffer, 5);
-            if(buffer[3] == 'S') {
-                SRX=1;
-                wait_ms(2);
-                UL_rR=sensorvalue(urR);
-                SRX=0;
-                wait_ms(2);
-                
-                SRX=1;
-                wait_ms(2);
-                UL_R=sensorvalue(uR);
-                SRX = 0;
-                wait_ms(2);
-                
-                SRX=1;
-                wait_ms(2);
-                UL_F=sensorvalue(uF);
-                SRX=0;
-                wait_ms(2);
-                
-                SRX=1;
-                wait_ms(2);
-                UL_L=sensorvalue(uL);
-                SRX=0;
-                wait_ms(2);
-                
-                SRX=1;
-                wait_ms(2);
-                UL_rL=sensorvalue(urL);
-                SRX=0;
-                wait_ms(2);
-                
-                SRX=1;
-                wait_ms(2);
-                UL_B=sensorvalue(uB);
-                SRX=0;
-                
-                bt.lock();
-                stdio_mutex.lock();
-                heading=heading*11.375;
-                bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
-                          dx/256,dx%256,dy/256,dy%256,\
-                          (heading)/256,(heading)%256,\
-                          stall, bump,\
-                          UL_rR/256,UL_rR%256,\
-                          UL_R/256,UL_R%256, \
-                          UL_F/256,UL_F%256,\
-                          UL_L/256,UL_L%256,\
-                          UL_rL/256,UL_rL%256,\
-                          UL_B/256,UL_B%256);
-                dx=0;
-                dy=0;
-                stdio_mutex.unlock();
-                bt.unlock();
-            } else if (buffer[3] == 'R') {
-                Thread::wait(20);
-                if(bt.readable()) {
-                    bt.gets(buffer, 10);
-                    linear_velocity_value = buffer[4]<<8|buffer[3];
-                    linear_velocity_direction= buffer[5];
-                    rotational_velocity_value = buffer[7]<<8|buffer[6];
-                    rotational_velocity_direction= buffer[8];
-                    if( linear_velocity_direction==0x01) {
-                        Lspeed=lMotor;
-                        Rspeed=rMotor;
-                    } else if( linear_velocity_direction==0x10) {
-                        Lspeed=-lMotor;
-                        Rspeed=-rMotor;
-                    }
-                    if( rotational_velocity_direction==0x01 && linear_velocity_value !=0) {
-                        
-                       
-                        motor_control((Lspeed*linear_velocity_value/35)-10,(Rspeed*linear_velocity_value/35) + 10);
-                        
-                    } else if( rotational_velocity_direction==0x10 && linear_velocity_value !=0) {
-                      motor_control((Lspeed*linear_velocity_value/35)+10,(Rspeed*linear_velocity_value/30) -10);
-                    }
-                    else
-                      motor_control((Lspeed*linear_velocity_value/30),(Rspeed*linear_velocity_value/30));
-                    
-                }
-            }
-            //memset(buffer, 0, sizeof buffer);
-        }
-
         if(ldrread1()>0.6) {
         }
         if(ldrread2()>0.6) {
@@ -156,6 +106,7 @@
         } else {
             Led_off();
         }
-        Thread::wait(20);
+        ultrasonic_run();
+        Thread::yield();
     }
-}
\ No newline at end of file
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shell/bt_shell.cpp	Sun Oct 12 13:33:19 2014 +0000
@@ -0,0 +1,86 @@
+#include "bt_shell.h"
+//#include "keybindings.h"
+//the following functions are for the python interface
+
+//save a struct that indicates which data is to be returned
+Timer tt;
+interface iface;
+int linear_velocity_value ;
+int linear_velocity_direction;
+int rotational_velocity_value ;
+int rotational_velocity_direction;
+int bit_size=20;
+int thetha1=300;
+int thetha=0;
+int stall=0;
+int bump=1;
+int Lspeed=1;
+int Rspeed=1;
+int k=0;
+int char_received=0;
+DigitalOut myreset(PTA20);
+Timeout reset_pgm;
+//mandatory tiny shell output function
+void print_all()
+{
+    bt.lock();
+    stdio_mutex.lock();
+    heading=heading*11.375;
+    bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
+              dx/256,dx%256,dy/256,dy%256,\
+              (heading)/256,(heading)%256,\
+              stall, bump,\
+              UL_rR/256,UL_rR%256,\
+              UL_R/256,UL_R%256, \
+              UL_F/256,UL_F%256,\
+              UL_L/256,UL_L%256,\
+              UL_rL/256,UL_rL%256,\
+              UL_B/256,UL_B%256);
+    dx=0;
+    dy=0;
+    stdio_mutex.unlock();
+    bt.unlock();
+}
+void bt_shell_run()
+{
+    char buffer[12];
+    bt.lock();
+    if(bt.readable()) {
+        bt.gets(buffer,5);
+        char_received=buffer[2];
+        bt.unlock();
+        if(buffer[3]=='R') {
+            bt.gets(buffer,char_received);
+            linear_velocity_value = buffer[0]<<8|buffer[1];
+            linear_velocity_direction= buffer[2];
+            rotational_velocity_value = buffer[3]<<8|buffer[4];
+            rotational_velocity_direction= buffer[5];
+            if( linear_velocity_direction==0x01) 
+            {
+                 Lspeed=-lMotor;
+                Rspeed=-rMotor;
+            } else if( linear_velocity_direction==0x10) {
+                Lspeed=+lMotor;
+                Rspeed=+rMotor;
+            }
+            if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) {
+                motor_control(Lspeed*60,Rspeed*0);
+               
+            } else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) {
+               motor_control(Lspeed*0,Rspeed*60);
+            } else{
+                motor_control((Lspeed*linear_velocity_value/4),(Rspeed*linear_velocity_value/4));
+                
+                }
+        } else if(buffer[0] == 'P'  && buffer[1]== 'O') {
+          software_interuupt=1;
+            myreset=0;
+        }
+        else
+        {
+            bt.rxBufferFlush();
+        }
+    }
+    bt.unlock();
+    print_all();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shell/bt_shell.h	Sun Oct 12 13:33:19 2014 +0000
@@ -0,0 +1,22 @@
+#ifndef BT_shell_H
+#define BT_shell_H
+#include "robot.h"
+#include "ultrasonic.h"
+
+extern "C" void mbed_reset();
+void bt_shell_run();
+void print_all();
+struct interface{
+    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