Diff: main.cpp

Revision:

30:3cfa8d7f84de

Parent:

29:43056f5cd0db

--- a/main.cpp	Fri Jun 01 01:54:04 2018 +0000
+++ b/main.cpp	Mon Jun 18 02:51:56 2018 +0000
@@ -28,14 +28,12 @@
 DigitalOut xshut1(XS1);
 DigitalOut xshut2(XS2);
 DigitalOut xshut3(XS3);
-
 DigitalOut xshut4(XS4);
 DigitalOut xshut5(XS5);
 
-VL53L0X sensor(&ItC_ranging, &time_r);
+VL53L0X sensor1(&ItC_ranging, &time_r);
 VL53L0X sensor2(&ItC_ranging, &time_r);
 VL53L0X sensor3(&ItC_ranging, &time_r);
-
 VL53L0X sensor4(&ItC_ranging, &time_r);
 VL53L0X sensor5(&ItC_ranging, &time_r);
     
@@ -43,9 +41,23 @@
 struct RangeData{
     uint16_t fwd,right,right_d,left_d,left;
     double theta_idx;
+    
+    uint16_t range[5];
+    float sensorValues[5][5];    
     };
 RangeData RangeSensor;
 
+/*LV*/
+
+pidBearing PID_B; //PID control for bearing
+motionPlanning MP;
+//void ranging_thread(void const *n); 
+void motionPlanning_thread(void const *n);
+
+/*LV*/
+
+
+
 //** IMU **
 /* THREAD */
 void imu_thread(void const *n);
@@ -71,7 +83,7 @@
 Timer motorControl_t;
 float rpm_cmd[2]; //drive motor rpm command
 float rpm_compensated[2]; //rpm command compensated by acc limit
-float targetAcceleration = 150.0; //RPM/s acceleration
+float targetAcceleration = 350.0; //RPM/s acceleration
 float pwm_cmd[2]; //drive motor pwm command
 bool motor_enable = 0;
 
@@ -235,7 +247,12 @@
 //        //** start OdometryUpdate function as Thread **
     Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
     debugprint.printf("* Odometry routine started *\n");
-//    
+//
+
+    //** start Motion Planning funtion as Thread **
+    Thread MotionPlanning_function(motionPlanning_thread, NULL, osPriorityNormal);
+    debugprint.printf("* Motion Planning routine started *\n"); 
+        
 //        //** start MotorControl function as Thread **
     Thread MotorControl_function(motorControl_thread, NULL, osPriorityHigh); // should be osPriorityHigh
     debugprint.printf("* Motor control routine started *\n");
@@ -284,11 +301,11 @@
 debugprint.printf("Sensor 1: \nXSHUT ON\n");
 Thread::wait(W);
 
-sensor.init();
+sensor1.init();
 debugprint.printf("S1 Initialized...\n");
 Thread::wait(W);
 
-sensor.setAddress((uint8_t)22);
+sensor1.setAddress((uint8_t)22);
 debugprint.printf("S1 Address set...\n");
 
 
@@ -340,13 +357,13 @@
 
 //////////////////////////////////////////////////
 
-sensor.startContinuous();
+sensor1.startContinuous();
 sensor2.startContinuous();
 sensor3.startContinuous();
 
 sensor4.startContinuous();
 sensor5.startContinuous();
-debugprint.printf("S5 Address set... %u \n",sensor.readRangeContinuousMillimeters());
+debugprint.printf("S5 Address set... %u \n",sensor1.readRangeContinuousMillimeters());
 debugprint.printf("S5 Address set... %u \n",sensor2.readRangeContinuousMillimeters());
 debugprint.printf("S5 Address set... %u \n",sensor3.readRangeContinuousMillimeters());
         
@@ -436,23 +453,67 @@
  */
 void static_ranging_thread(void const *n)
 {
-    while(true)
+//    while(true)
+//    {
+////      if(imu.imu_stabilized[1] ==1){   
+//        RangeSensor.right   =  sensor.readRangeContinuousMillimeters();
+//        RangeSensor.right_d = sensor2.readRangeContinuousMillimeters();
+//        RangeSensor.fwd     = sensor3.readRangeContinuousMillimeters();
+//        
+//        RangeSensor.left_d  = sensor4.readRangeContinuousMillimeters();
+//        RangeSensor.left    = sensor5.readRangeContinuousMillimeters(); 
+////        if (sensor.timeoutOccurred() || sensor2.timeoutOccurred() || sensor3.timeoutOccurred())  
+////            debugprint.printf(" TIMEOUT\r\n"); 
+////        }
+//      Thread::wait(10);
+//      }   
+
+
+
+    while(1)
     {
-//      if(imu.imu_stabilized[1] ==1){   
-        RangeSensor.right   =  sensor.readRangeContinuousMillimeters();
-        RangeSensor.right_d = sensor2.readRangeContinuousMillimeters();
-        RangeSensor.fwd     = sensor3.readRangeContinuousMillimeters();
+      //if(imu.imu_stabilized[1] ==1){
+                        
+         for (int n=0; n<5; n++){//stores 5 values from each sensor       
+            RangeSensor.sensorValues[0][n] = sensor1.readRangeContinuousMillimeters();//extreme right
+            RangeSensor.sensorValues[1][n] = sensor2.readRangeContinuousMillimeters();            
+            RangeSensor.sensorValues[2][n] = sensor3.readRangeContinuousMillimeters();
+            RangeSensor.sensorValues[3][n] = sensor4.readRangeContinuousMillimeters(); 
+            RangeSensor.sensorValues[4][n] = sensor5.readRangeContinuousMillimeters();}//extreme left   
         
-        RangeSensor.left_d  = sensor4.readRangeContinuousMillimeters();
-        RangeSensor.left    = sensor5.readRangeContinuousMillimeters(); 
-//        if (sensor.timeoutOccurred() || sensor2.timeoutOccurred() || sensor3.timeoutOccurred())  
-//            debugprint.printf(" TIMEOUT\r\n"); 
-//        }
-      Thread::wait(10);
-      }           
+        
+        for (int i=0; i<5; i++){ 
+            RangeSensor.range[i] = generalFunctions::moving_window(RangeSensor.sensorValues[i], 5);}  
+            
+      //}      
+            Thread::wait(1.0);  
+              
+    }
+            
 }
  
-
+/**/
+/**
+ * motion planning loop as an individual thread
+ */
+ 
+ void motionPlanning_thread(void const *n){
+        
+        while(true) {
+            
+            if(imu.imu_stabilized[1] ==1){
+                            
+                            MP.planTrack(RangeSensor.range, 
+                                target_waypoint,
+                                localization.position,
+                                imu.Pose[2],
+                                PID_B.robotFrame, PID_B.Error, imuTime);
+                }
+            Thread::wait(4.0);
+            
+            }
+     }
+/**/
 
 void odometry_thread(void const *n)
 {
@@ -484,77 +545,48 @@
 {
     motorControl_t.start();
 
-    float pitch_th, pitch_om, yaw_th, yaw_om;
-    float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
     float W_l, W_r;
+    
+
 
     while(true) {
-//        debugprint.printf("%.2f Running 3 motorControl \n", imuTime);
+        
+
         //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
         if(imu.imu_stabilized[1] ==1) {
-
-            pitch_th = DEG_TO_RAD(imu.Pose[0]);
-            if(pitch_th < -1*M_PI)   pitch_th += 2*M_PI;
-            if(pitch_th > M_PI)   pitch_th -= 2*M_PI;
-            if(pitch_th < -1*M_PI)   pitch_th += 2*M_PI;
             
-            
-            pitch_om = DEG_TO_RAD(imu.AngVel[0]);
-
-            yaw_th = DEG_TO_RAD(imu.Pose[2]);
-            
-            yaw_om = DEG_TO_RAD(imu.AngVel[2]);
-
-            wheelTh_l = odometry.revolutions[0]*(-2*M_PI);
-            wheelTh_r = odometry.revolutions[1]*(-2*M_PI);
+        PID_B.setSpeedChange(   &W_l, &W_r,
+                                    MP.plan,
+                                    localization.position,
+                                    imu.Pose[2], 
+                                    &MP.linearSpeed, target_velocity, MP.kp, MP.kd, MP.GTGTrue); //new controller              
 
-            wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI);
-            wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI);
-
-//            Rasp.printf("%.2f \n", purePursuit_velocity);
-            ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0);
-            ref_gamma = purePursuit_gamma;
-            ref_dgamma = purePursuit_omega;
+            if(motor_enable == 1) {
+                rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
+                rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
+//                rpm_cmd[0]=700;
+//                rpm_cmd[1]=700;
 
-            attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(),
-                                               pitch_th, pitch_om, yaw_th , yaw_om,
-                                               wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r,
-                                               0, ref_dbeta, ref_beta,
-                                               0, ref_dtheta, ref_theta,
-                                               0, ref_dgamma, ref_gamma,
-                                               &u1, &u2);
-            
+//                if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) ) //was 500 and 475
+//                    rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]);
+//                else if(generalFunctions::abs_f(rpm_cmd[0]) <= 100.0)
+//                    rpm_cmd[0] = 0;
+//
+//                if( (generalFunctions::abs_f(rpm_cmd[1]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 100.0) )
+//                    rpm_cmd[1] = 475.0*generalFunctions::sign_f(rpm_cmd[1]);
+//                else if(generalFunctions::abs_f(rpm_cmd[1]) <= 100.0)
+//                    rpm_cmd[1] = 0;
 
 
-//            Rasp.printf("Reach %d \n", waypointReached_flag);
-//            Rasp.printf("Finsih %d \n", waypointSetFinish_flag);
-//            if(waypointSetFinish_flag == 0 && waypointReached_flag == 0 ) {
-//              Rasp.printf("Motor enable: %d \n", motor_enable);
-              if (motor_enable == 1){
-                rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);// Virgo
-                rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
-//               Rasp.printf("%.2f \n", rpm_cmd[0]);
-//                rpm_cmd[0]=W_l*60/(2*M_PI)*(1.0); //Orion
-//                rpm_cmd[1]=W_r*60/(2*M_PI)*(1.0);
                 
-//                rpm_cmd[0]=0;
-//                rpm_cmd[1]=0;
-
-                if( (generalFunctions::abs_f(rpm_cmd[0]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 10.0) )
-                    rpm_cmd[0] = 47.5*generalFunctions::sign_f(rpm_cmd[0]);
-                else if(generalFunctions::abs_f(rpm_cmd[0]) <= 10.0)
-                    rpm_cmd[0] = 0;
-
-                if( (generalFunctions::abs_f(rpm_cmd[1]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 10.0) )
-                    rpm_cmd[1] = 47.5*generalFunctions::sign_f(rpm_cmd[1]);
-                else if(generalFunctions::abs_f(rpm_cmd[1]) <= 10.0)
-                    rpm_cmd[1] = 0;
-
                 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read());
                 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read());
-
+                
+                //rpm_compensated[0]= rpm_cmd[0];
+                //rpm_compensated[1]= rpm_cmd[1];
 
-            } else {
+            } 
+            else {
                 rpm_cmd[0]=0;
                 rpm_cmd[1]=0;
                 
@@ -567,7 +599,8 @@
 
             drive.setPWM_L(pwm_cmd[0]);
             drive.setPWM_R(pwm_cmd[1]);
- 
+                     
+            
         }
 
         motorControl_t.reset();
@@ -587,9 +620,13 @@
 ////                Rasp.printf("%.2f,%.2f,%.2f; ",imu.AngVel[0], imu.AngVel[1], imu.AngVel[2]); //ang vel x,y,z
 ////                Rasp.printf("%.2f,%.2f,%.2f; ",imu.LinAcc[0], imu.LinAcc[1], imu.LinAcc[2]); //Linear acc
                 Rasp.printf("%.2f,%.2f; ",localization.position[0], localization.position[1]); //Localization X,Y
-                Rasp.printf("%u,%u,%u,%u,%u; ", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+                Rasp.printf("%u,%u,%u,%u,%u; ", RangeSensor.range[4], RangeSensor.range[3], RangeSensor.range[2], RangeSensor.range[1], RangeSensor.range[0]);
+                
                 Rasp.printf("%.2f,%.2f; ",trilateration.robot_pos.x, trilateration.robot_pos.y); //uwb position
                 Rasp.printf("%d,%d,%d; ",trilateration.range_array[0], trilateration.range_array[1], trilateration.range_array[2]); //uwb range data
+                
+                //Rasp.printf("%.2f; ",PID_B.robotFrame); //LV RF
+               
 //                Rasp.printf("%.2f,%.2f; ",odometry.revolutions[0] * 2*M_PI, odometry.revolutions[1] * 2*M_PI); //Wheel Position L,R
                 Rasp.printf("%.2f,%.2f; ",odometry.rpm[0] * 2*M_PI / 60, odometry.rpm[1] * 2*M_PI / 60); //Wheel Speed L,R
                 Rasp.printf("%.2f,%.2f; ", target_waypoint[0], target_waypoint[1]); //Waypoint heading to
@@ -965,12 +1002,23 @@
 //        debugprint.printf("%.2f Running 7 purePursuit \n", imuTime);
         if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
             //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
-            purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
+            
+//            purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
+//
+//            if(purePursuit.robotFrame_targetDistance <= waypointZone)
+//                waypointReached_flag = 1;
+//            else
+//                waypointReached_flag = 0;
 
-            if(purePursuit.robotFrame_targetDistance <= waypointZone)
+/*#LV*/
+        PID_B.findRobotFrameDistance(target_waypoint, localization.position);   
+                  
+            if(PID_B.robotFrame <= waypointZone)
                 waypointReached_flag = 1;
             else
-                waypointReached_flag = 0;
+                waypointReached_flag = 0;  
+/*#LV*/ 
+
         }
         Thread::wait(imu_UpdatePeriodMS);
     }
@@ -985,7 +1033,7 @@
 //        debugprint.printf("%.2f waypoint cmd \n", imuTime);
         //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
         if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
-
+ 
 //            if(waypoint_index > totalWaypoints) {
 //                target_velocity = 0.0; //stop the robot
 //                waypointSetFinish_flag = 1;
@@ -995,7 +1043,7 @@
                 target_velocity = 0.0; //stop the robot;
                 motor_enable = 0;
             }
-
+ 
 //            debugprint.printf("waypointReached_flag = %d, waypoint_ready = %d \n", waypointReached_flag,waypoint_ready);
 //            debugprint.printf("target waypoint %.2f %.2f \n", target_waypoint[0], target_waypoint[1]);
             if(waypointReached_flag == 1 && waypointSetFinish_flag == 0 && waypoint_ready == 1) {
@@ -1003,7 +1051,8 @@
 //            if(waypointReached_flag == 1 && waypointSetFinish_flag == 0){
                 target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
                 target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
-                target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
+                //target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
+                target_velocity = 1.0;
                 //target_velocity = 90*(driveTrain_maxV/100.0);
                 waypoint_ready = 0;
 //                waypoint_index++;
@@ -1147,7 +1196,9 @@
 //        debug.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready);
 //        //debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
 //        debugprint.printf("Raspberry waypoint: %s \n\e[K", rasp_data);
-        debugprint.printf("Ranging: %u, %u, %u, %u, %u\n\e\K", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+
+        debugprint.printf("Ranging: %u, %u, %u, %u, %u\n\e\K", RangeSensor.range[4], RangeSensor.range[3], RangeSensor.range[2], RangeSensor.range[1], RangeSensor.range[0]);
+
 //        debugprint.printf("UWB ranging: %s %s %s\n\e[K", rangestring_array[0],rangestring_array[1], rangestring_array[2]);
         debugprint.printf("UWB ranging: %d %d %d \n\e[K", trilateration.range_array[0],trilateration.range_array[1],trilateration.range_array[2]);
         debugprint.printf("x : %f , y : %f , z : %f \n\e[K", trilateration.robot_pos.x, trilateration.robot_pos.y, trilateration.robot_pos.z);