Diff: robot.cpp

Revision:

1:282467cbebb3

Parent:

0:b74b6d70347d

Child:

2:37a19a9e5f2c

--- a/robot.cpp	Sun Oct 05 12:21:03 2014 +0000
+++ b/robot.cpp	Sat Oct 11 03:53:04 2014 +0000
@@ -12,8 +12,8 @@
 //*********************************CONSTRUCTOR*********************************//
 HC05 bt(tx_bt,rx_bt,EN_BT);
 //QEI wheel (PTA16, PTA17, NC, 24);
-QEI left (PTA16, PTA17, NC, 150, QEI::X4_ENCODING);
-QEI right (PTA14, PTA13, NC, 150, QEI::X4_ENCODING);
+QEI right (PTA16, PTA17, NC, 150, QEI::X4_ENCODING);
+QEI left (PTA14, PTA13, NC, 150, QEI::X4_ENCODING);
 //Serial bt(rx_bt,tx_bt);
 //MPU6050 mpu(PTE0, PTE1);
 DigitalOut myled(myledd);
@@ -33,38 +33,50 @@
 DigitalOut BIN1(MOT_BIN1_PIN);
 DigitalOut BIN2(MOT_BIN2_PIN);
 DigitalOut STBY(MOT_STBY_PIN);
-int rMotor = -1;
+
+DigitalOut SRX(PTB10);
+
+AnalogIn uL(ulL);
+AnalogIn uF(ulF);
+AnalogIn uR(ulR);
+AnalogIn urR(ulrR);
+AnalogIn urL(ulrL);
+AnalogIn uB(ulB);
+MPU6050 accelgyro;
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+int rMotor = 1;
 int lMotor = -1;
 int m_speed = 100;
 int speed;
 Mutex stdio_mutex; 
 int freq=0;
-/*
-double target_angle=0;
-double rz;          //Direction robot is facing
-double Irz;         //integral of the rotation offset from target. (Optionally) Used for PID control of direction
-double angle_origin;      //Angle of origin (can be changed later, or set if robot starts at known angle)
-bool AUTO_ORIENT; //if this flag is 1, the robot automatically orients itself to selected direction
-bool REMOTE_CONTROL;    //if this flag is 1, the robot will be controlled over bluetooth
-int acc[3];
-int gyr[3];
-bool MPU_OK;
-double timeNext;  
-int speed;
-int accdata[3];     //data from accelerometer (raw)
-int gyrodata[3];    //data from gyro (raw)
-   //double gyroCorrect; //= 3720;  //divide by this to get gyro data in RAD/SECOND. This is above as a #DEFINE
-int gyroOffset[3];  //Correction value for each gyroscope to zero the values.
-int accOffset[3];  //correction value for each accelerometer
-double dx = 0;          //The current displacement in the x-axis    (side-side)
-double dy = 0;          //The current displacement in the y-axis    (forward-back)
-double dz = 0;          //The current displacement in the z-axis    (up-down)
-double origin = 0; 
-int freq=0;
-*/ 
+Timer t;
+int heading=0;
+int last_time=0;
+int dy =0;
+int dx=0;
+float left_current_reading=0;
+float   right_current_reading= 0;
+float   left_change  = 0;
+float   right_change =0;
+float   left_prev_read=0;
+float   right_prev_read=0;
+int   delta_y=0;
+int   delta_x=0;
+float   delta_thetha=0;
+float   encoder_yaw =0;
+float   G_thetha=0;
+int Encoder_x=0;
+int Encoder_y=0;
+int dtheta=0;
+int r_time ()
+{
+    int mseconds = (int)time(NULL)+(int)t.read_ms();
+    return mseconds;
+}
 void initRobot()
 {
-
     key  = 0;
     //btSwitch = 1;
     bt.start();
@@ -72,6 +84,9 @@
     wait_ms(500);
     bt.baud(BaudRate_bt);
     myled = 1;
+    accelgyro.initialize();
+    t.start();
+    SRX = 0;
 }
 
 //*********************************MOTORS*********************************//
@@ -179,13 +194,63 @@
         //bt.printf("s;%lf;s\n\r",pulse_delay);
         //bt.unlock();
         buzzer=1;
-        Thread::wait(pulse_delay);
+        //Thread::wait(pulse_delay);
         buzzer=0;
-        Thread::wait(pulse_delay);
+        //Thread::wait(pulse_delay);
         }
    
     //freq = 150+(freq*10);
     //buzzer.period_us(1000000/freq);  // 4 second period
     //buzz.pulsewidth(2); // 2 second pulse (on)
     //buzzer.write(0.5f);  // 50% duty cycle
+}
+void Imu_yaw(void const *args)
+{
+while(true)
+
+{
+    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;
+        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;
+        stdio_mutex.unlock();
+        Thread:: wait(20);
+}    
+}
+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);
+}    
 }
\ No newline at end of file