Diff: robot.cpp

Revision:

2:37a19a9e5f2c

Parent:

1:282467cbebb3

Child:

3:3e3314102e56

--- a/robot.cpp	Sat Oct 11 03:53:04 2014 +0000
+++ b/robot.cpp	Sun Oct 12 13:33:06 2014 +0000
@@ -46,10 +46,10 @@
 int16_t ax, ay, az;
 int16_t gx, gy, gz;
 int rMotor = 1;
-int lMotor = -1;
+int lMotor = 1;
 int m_speed = 100;
 int speed;
-Mutex stdio_mutex; 
+Mutex stdio_mutex;
 int freq=0;
 Timer t;
 int heading=0;
@@ -70,6 +70,7 @@
 int Encoder_x=0;
 int Encoder_y=0;
 int dtheta=0;
+int software_interuupt;
 int r_time ()
 {
     int mseconds = (int)time(NULL)+(int)t.read_ms();
@@ -93,10 +94,9 @@
 void motor_control(int Lspeed, int Rspeed)
 {
     //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
-    if (!Lspeed && !Rspeed)     //stop//
-      {  STBY = 0;
-         }
-    else
+    if (!Lspeed && !Rspeed) {   //stop//
+        STBY = 0;
+    } else
         STBY = 1;
     //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
     if(Lspeed > 100) Lspeed = 100;
@@ -184,21 +184,20 @@
 */
 void pl_buzzer(void const *args)
 {
-         while(true)
-        {
-         stdio_mutex.lock();
+    while(true) {
+        stdio_mutex.lock();
         float pulse_delay = 150+((float)freq*10);
         pulse_delay= 1000/pulse_delay;
         stdio_mutex.unlock();
-       // bt.lock();
+        // bt.lock();
         //bt.printf("s;%lf;s\n\r",pulse_delay);
         //bt.unlock();
         buzzer=1;
         //Thread::wait(pulse_delay);
         buzzer=0;
         //Thread::wait(pulse_delay);
-        }
-   
+    }
+
     //freq = 150+(freq*10);
     //buzzer.period_us(1000000/freq);  // 4 second period
     //buzz.pulsewidth(2); // 2 second pulse (on)
@@ -206,51 +205,47 @@
 }
 void Imu_yaw(void const *args)
 {
-while(true)
+    while(true)
 
-{
-    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-    int m_seconds = r_time();
+    {
+        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        int m_seconds = r_time();
         float dt = ((float)(m_seconds-last_time))/1000;
-         last_time=m_seconds;
+        last_time=m_seconds;
         if ((gz)<800&& gz>-800) {
             gz=0;
         }
         stdio_mutex.lock();
-         heading = heading + (dt*gz)*3/4/100;
-         if(heading>360)
-         heading= heading -360;
-         else if (heading <0)
-         heading = heading +360;
+        heading = heading + (dt*gz)*3/4/100;
+        if(heading>360)
+            heading= heading -360;
+        else if (heading <0)
+            heading = heading +360;
         stdio_mutex.unlock();
-        Thread:: wait(20);
-}    
+        Thread:: wait(50);
+    }
 }
 void encoder_thread(void const *args)
 {
-while(true)
-{
-   left_current_reading=left.getPulses()*(-1)/5;
-   right_current_reading= right.getPulses()/5;
-   left_change  = left_current_reading- left_prev_read;
-   right_change =right_current_reading- right_prev_read;
-   left_prev_read=left_current_reading;
-   right_prev_read=right_current_reading;
-   delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2;
-   delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2;
-   delta_thetha=atan((right_change-left_change)/100);
-   encoder_yaw =encoder_yaw+delta_thetha;
-   G_thetha=encoder_yaw*180/M_PI;           //global thetha, overall
-   Encoder_x=Encoder_x+delta_x;
-   Encoder_y=Encoder_y+delta_y;
-   stdio_mutex.lock();
-   dx=delta_x+dx;
-   dy=delta_y+dy;
-   dtheta=delta_thetha*180/M_PI;
-   stdio_mutex.unlock();
-   //bt.lock();
-   //bt.printf("%0.2lf\t%0.2lf;\t%d;\t%d;\t%d;\t%d;\t%lf;\t%lf;\t :s\n\r",left_current_reading,right_current_reading,Encoder_x,Encoder_y,delta_y,delta_x,delta_thetha,G_thetha);
-   //bt.unlock();
-Thread:: wait(50);
-}    
+    while(true) {
+        left_current_reading=left.getPulses()*(-1)/5;
+        right_current_reading= right.getPulses()/5;
+        left_change  = left_current_reading- left_prev_read;
+        right_change =right_current_reading- right_prev_read;
+        left_prev_read=left_current_reading;
+        right_prev_read=right_current_reading;
+        delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2;
+        delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2;
+        delta_thetha=atan((right_change-left_change)/100);
+        encoder_yaw =encoder_yaw+delta_thetha;
+        G_thetha=encoder_yaw*180/M_PI;           //global thetha, overall
+        Encoder_x=Encoder_x+delta_x;
+        Encoder_y=Encoder_y+delta_y;
+        stdio_mutex.lock();
+        dx=delta_x+dx;
+        dy=delta_y+dy;
+        dtheta=delta_thetha*180/M_PI;
+        stdio_mutex.unlock();
+        Thread:: wait(50);
+    }
 }
\ No newline at end of file