Diff: orion_main.cpp

Revision:

27:2db168d9fb18

Parent:

26:32eaf3c3ac2e

Child:

28:39d694b0e998

--- a/orion_main.cpp	Thu Mar 15 09:14:43 2018 +0000
+++ b/orion_main.cpp	Thu Mar 15 10:14:47 2018 +0000
@@ -34,11 +34,37 @@
  */
 #include "main.h"
 #include "globalExterns.h"
+#include "VL53L0X.h"
+#define W 0.1
 
 /**
  * Functions, Threads & General Definitions
  */
 //*****************************************************************************
+
+//** Ranging **//
+I2C ItC_ranging(i2c2_SDA, i2c2_SCL);
+Timer time_r;
+
+DigitalOut xshut1(xs1);
+DigitalOut xshut2(xs2);
+DigitalOut xshut3(xs3);
+DigitalOut xshut4(xs4);
+DigitalOut xshut5(xs5);
+
+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);
+
+
+struct RangeData{
+     uint16_t exleft, left, front, right, exright;;
+    };
+RangeData RangeSensor; 
+
+
 //** Drivetrain **
 motorDriver drive;  //motor drive train
 odometer odometry;  //odometer function
@@ -81,18 +107,7 @@
 
 /* THREAD */
 void imu_thread(void const *n);
-/*   **   */
-//-------------
 
-//** Communications **
-//nRF24NetworkHandler comm;  //nRF24 radio and network layer handler function
-//uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
-//uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status,
-
-//cmdParser wirelessCmd;
-
-
-/*   **   */
 //-------------
 
 //** Power Monitor **
@@ -137,29 +152,8 @@
 
 //** Attitude control **
 attitudeControl attitudeControl;
-//pidAttitudeControl pidPitchControl;
-//pidAttitudeControl pidRollControl;
 
 float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
-
-/* THREAD */
-
-/*   **   */
-//-------------
-
-
-/*   **   */
-//-------------
-
-////** Robot data recorder **
-//RDR virgoRDR;
-//uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled
-
-/* THREAD */
-
-/*   **   */
-//-------------
-
 //** Declarations of misc functions **
 Serial Debug(uart_TX, uart_RX); //Debug serial port
 DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication
@@ -167,8 +161,9 @@
 /* THREAD */
 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
 void print_thread(void const *n); //Debug printing thread
+void ranging_thread(void const *n); //ranging VL53L0X
 /*   **   */
-//-------------
+
 //-----------------------------------------------------------------------------
 
 
@@ -206,6 +201,85 @@
     Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
     Debug.printf("* Waypoint commander routine started *\n");
 
+
+    
+       
+    xshut1 = 0;
+    xshut2 = 0;
+    xshut3 = 0;
+    xshut4 = 0;
+    xshut5 = 0;
+
+    Thread::wait(W);
+
+/////////////////////////    
+    xshut1 = 1;
+    Debug.printf("Sensor 1: \nXSHUT ON\n");
+    Thread::wait(W);
+     
+    sensor1.init();
+    Debug.printf("S1 Initialized...\n");
+    Thread::wait(W);
+     
+    sensor1.setAddress((uint8_t)23);
+    Debug.printf("S1 Address set...\n");   
+/////////////////////////      
+    xshut2 = 1;
+    Debug.printf("Sensor 2: \nXSHUT ON\n");
+    Thread::wait(W);
+     
+    sensor2.init();
+    Debug.printf("S2 Initialized...\n");
+    Thread::wait(W);
+     
+    sensor2.setAddress((uint8_t)25);
+    Debug.printf("S2 Address set...\n");
+/////////////////////////      
+    xshut3 = 1;
+    Debug.printf("Sensor 3: \nXSHUT ON\n");
+    Thread::wait(W);
+     
+    sensor3.init();
+    Debug.printf("S3 Initialized...\n");
+    Thread::wait(W);
+     
+    sensor3.setAddress((uint8_t)27);
+    Debug.printf("S3 Address set...\n");   
+/////////////////////////      
+    xshut4 = 1;
+    Debug.printf("Sensor 4: \nXSHUT ON\n");
+    Thread::wait(W);
+     
+    sensor4.init();
+    Debug.printf("S4 Initialized...\n");
+    Thread::wait(W);
+     
+    sensor4.setAddress((uint8_t)29);
+    Debug.printf("S4 Address set...\n");
+/////////////////////////    
+    xshut5 = 1;
+    Debug.printf("Sensor 5: \nXSHUT ON\n");
+    Thread::wait(W);
+     
+    sensor5.init();
+    Debug.printf("S5 Initialized...\n");
+    Thread::wait(W);
+     
+    sensor5.setAddress((uint8_t)22);
+    Debug.printf("S5 Address set...\n");    
+    
+/////////////////////////      
+    sensor1.startContinuous(); 
+    sensor2.startContinuous(); 
+    sensor3.startContinuous(); 
+    sensor4.startContinuous();
+    sensor5.startContinuous();     
+    //** start Ranging funtion as Thread **
+    Thread Ranging_function(ranging_thread, NULL, osPriorityNormal);
+    Debug.printf("* Ranging routine started *\n");      
+    
+    
+
     //** start Debug print loop as a thread **
     Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
     Debug.printf("* Print loop started *\n\n\n");
@@ -264,6 +338,33 @@
     }
 }
 
+
+/**
+ * Ranging sensor loop as an individual thread
+ */
+ 
+void ranging_thread(void const *n)
+{
+
+        
+
+    
+    while(1)
+    {
+      if(imu.imu_stabilized[1] ==1){            
+            RangeSensor.exleft = sensor1.readRangeContinuousMillimeters();
+            RangeSensor.left = sensor2.readRangeContinuousMillimeters();            
+            RangeSensor.front = sensor3.readRangeContinuousMillimeters();
+            RangeSensor.right = sensor4.readRangeContinuousMillimeters(); 
+            RangeSensor.exright = sensor5.readRangeContinuousMillimeters();                                   
+            //debugprint.printf("in ranging %u \n\e[K", RangeSensor.left);
+      }      
+            Thread::wait(1.0);  
+              
+    }
+}
+
+
 /**
  * odometry update loop as an individual thread
  */
@@ -317,43 +418,10 @@
         //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);
-
-            wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI);
-            wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI);
-
-            //ref_dtheta = rpm_smc*(-2*M_PI/60.0); //*generalFunctions::abs_f(sin(2*M_PI*imuTime/(15*2)));
-            //ref_theta = ref_dtheta * imuTime;
-            //ref_gamma = DEG_TO_RAD(30.0) ;//* sin(2*M_PI*imuTime/(30*2));
-            
-            ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0);
-            ref_gamma = purePursuit_gamma;
-            ref_dgamma = purePursuit_omega;
-
-//            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);
             
             PID_B.setSpeedChange(   &W_l, &W_r, &speedChange, &aError, target_waypoint[0], target_waypoint[1], 
                         localization.position[0], localization.position[1], imu.Pose[2], purePursuit.robotFrame_targetDistance,
-                        waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3 ); //new controller
+                        waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3, 3.5); //new controller
 
 
 
@@ -484,7 +552,7 @@
         Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, BearingToWaypoint %.4f \n\e[K", purePursuit.robotFrame_targetDistance, bearingToWaypoint);
         Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", aError, speedChange, k3);
         Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
-
+        Debug.printf("Ranging: %u, %u, %u, %u, %u \n\e[K", RangeSensor.exleft, RangeSensor.left, RangeSensor.front, RangeSensor.right, RangeSensor.exright);
 
 
         Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);