Diff: main.cpp

Revision:

5:daa981fdd231

Parent:

4:26006d815db0

--- a/main.cpp	Fri Aug 24 21:21:46 2018 +0000
+++ b/main.cpp	Fri Jun 14 08:19:00 2019 +0000
@@ -2,35 +2,208 @@
  * rosserial Subscriber Example
  * Blinks an LED on callback
  */
+ 
 #include "mbed.h"
 #include <ros.h>
 #include <std_msgs/String.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Float32.h>
+#include "Servo.h"
+#include "MPU6050.h"
+//#include "esc.h"
+//#include "MbedJSONValue.h"
 
+#define spd 60
+
+
+//Serial uart(PA_11, PA_12, 57600);
 ros::NodeHandle nh;
 DigitalOut myled(LED1);
+//PWM
+PwmOut sv(D5);
+PwmOut esc1(D9);
 
-std_msgs::String commandRead;
+std_msgs::Float32 mpu6050_yaw;
+std_msgs::Int8 commandRead;
 ros::Publisher chatter("chatter", &commandRead);
+ros::Publisher yaw_value("yaw_value", &mpu6050_yaw);
+
+int SPEED=0;
+int ANG=0;
+char DIR1='S';
+int Num = 0;
+char inData[80];
+float dt,Yaw, lgyroZ=0;
+int past_ANG = 90;
+int gyroZ_raw= 0;
+float default_PWM = 0.15;
+
+MPU6050 mpu6050;
+Timer t;
+ 
+void MPU6050_calculation()
+{
+    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) 
+    {  // check if data ready interrupt
+        mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+        mpu6050.getGres();
+        gyroZ_raw = (int)gyroCount[2]*gRes; // - gyroBias[2];
+    }
+    Now = t.read_us();
+    dt = (float)((Now - lastUpdate)/1000000.0f) ;
+    lastUpdate = Now;
+    Yaw += ((lgyroZ+gyroZ_raw)/2)*dt;
+    lgyroZ = gyroZ_raw;
+    mpu6050_yaw.data=Yaw;    
+}
 
-// /*
-void handlerFunction(const std_msgs::String& commandSend){
+void handlerFunction(const std_msgs::Int8& commandSend){
+    
     commandRead = commandSend;
+    switch (commandSend.data)
+    {
+        case 1: //forward
+            SPEED =spd;
+            DIR1 = 'F';
+            ANG = 90;  
+            break;
+        case 2: //strong left
+            SPEED =spd;
+            DIR1 = 'F';
+            ANG = 45;
+            break;            
+        case 3:  //strong right
+            SPEED =spd;
+            DIR1 = 'F';
+            ANG = 135;
+            break;    
+        case 4: // weak left
+            SPEED=spd;
+            ANG = 75;
+            DIR1 = 'F';
+            break;
+        case 5: //weak right 
+            SPEED = spd;
+            DIR1 = 'F';
+            ANG =105;
+            break;
+        case 6: // mid left
+            SPEED = spd;
+            DIR1 = 'F';
+            ANG =60;
+            break;
+        case 7: //mid right 
+            SPEED = spd;
+            DIR1 = 'F';
+            ANG =120;
+            break;
+        case 8: //stop 
+            SPEED = 1;
+            DIR1  ='S';
+            ANG   = 90;
+            break;
+        default :
+            SPEED = 1;
+            DIR1  ='S';
+            ANG   = 90;
+            break;
+    }
+    if (SPEED) 
+    { //assign speed value to Motor_PWM
+        float getPWM = SPEED/5000.0;
+            if(DIR1=='F')
+            {
+                for(int i=0; i<=SPEED; i++)
+                {
+                    esc1 = (default_PWM + getPWM*i/SPEED);
+                   //wait_ms(20);
+                }
+            }
+            else if(DIR1=='S')
+            {
+                esc1 = default_PWM;
+               // wait_ms(20); 
+            }
+            else if(DIR1=='B')
+            {
+                for(int i=0; i<=SPEED; i++)
+                {
+                    esc1 = (default_PWM - getPWM*i/SPEED);
+                    //wait_ms(20);                
+                }
+            }
+    }
+    
+    if (ANG)
+    {   //assign ANG value to the servo motor angle
+        float Angle_Value = 0;
+        if(ANG>=past_ANG)
+        {
+            for(int i=past_ANG; i<=ANG; i++)
+            {   
+                Angle_Value = 0.05+i/90.0*0.1;
+                sv.write(Angle_Value);
+                //wait(0.02);    
+            }
+        }
+        else if(past_ANG >= ANG)
+        {
+            for(int i=past_ANG; i>=ANG; i--)
+            {
+                Angle_Value = 0.05+i/90.0*0.1;
+                sv.write(Angle_Value);
+                //wait(0.02);
+            }
+        }
+        past_ANG = ANG;
+        //wait(0.02);
+    }   
+    
 }
-// */
 
-ros::Subscriber<std_msgs::String> sub("cmd_vel", &handlerFunction);
+ros::Subscriber<std_msgs::Int8> sub("/data_msg", &handlerFunction);
 
 int main() {
+    
     nh.initNode();
     nh.subscribe(sub);
+    
     nh.advertise(chatter);
+    nh.advertise(yaw_value);
+    
+    esc1.period(0.01f);    
+    esc1.write(default_PWM);
+    sv.period(0.01f);
+    sv.write(0.15);
+    i2c.frequency(400000);
+    t.start();
+    
+    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // read WHO_AM_I register
     
-    char initialValue[2] = "X";
-    commandRead.data= initialValue;
+    if (whoami == 0x68)
+    {
+        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
 
+        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
+        {
+            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+            mpu6050.initMPU6050();
+        } 
+    } 
+    else {
+        while(1) ; // Loop forever if communication doesn't happen
+    }
+    wait_ms(100);
+    
     while (1) {
-        chatter.publish( &commandRead);
+        //Drive_Contrl(); 
+        MPU6050_calculation();
+        
+        yaw_value.publish(&mpu6050_yaw);       
+        chatter.publish(&commandRead);
+        
         nh.spinOnce();
-        wait_ms(10);
+        wait_ms(100);
     }
-}
+}
\ No newline at end of file