Diff: main.cpp

Revision:

9:5320c2dfb913

Parent:

7:05ea1c004b49

Child:

13:0091da3021df

--- a/main.cpp	Mon Oct 25 18:48:17 2021 +0000
+++ b/main.cpp	Tue Oct 26 01:22:05 2021 +0000
@@ -18,23 +18,28 @@
 Ticker motorLoop;
 Ticker fault_detector; 
 Timer t;
+Ticker controlUpdate; 
 
+void PID (void) {
+    steering_control(); 
+    PI();
+    };
 int main() { 
     state_update();
-   
     //Delcare Onboard LED with blue color
     DigitalOut led_b(LED_BLUE);
     //Set the period of the servo motor control signal
     servo_motor_pwm.period(1/SERVO_MOTOR_FREQ);
     motorLeft.period(1/freq);
     motorRight.period(1/freq);
+    controlUpdate.attach(&PID, TI);
     //Center the servo motor
-    servo_motor_pwm.write(CENTER_DUTY_CYCLE);
-
+    //servo_motor_pwm.write(CENTER_DUTY_CYCLE);
+    //controlUpdate.attach(&PID, TI);
     //Start the control systm using a Ticker object
-      steering_control_ticker.attach(&steering_control, TI);
-      motorLoop.attach(&PI,TI);
-      fault_detector.attach(&fault_check, 0.02);
+     // steering_control_ticker.attach(&steering_control, TI);
+     // motorLoop.attach(&PI,TI);
+  //    fault_detector.attach(&fault_check, 0.02);
       //bt.attach(&btReceive);
     
     // call landmark_counter wjen a landmark is detected 
@@ -58,24 +63,25 @@
      
      pc.printf("\n\n");
 
-     pc.printf("\r\nSteering enabled? : %d",steering_enabled);
-     pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
-     pc.printf("\r\nLeft duty cycle : %f ", left_distance_sensor.read());
-     pc.printf("\r\nRight voltage : %f"      , right_distance_sensor.read());
-     pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
-       fault_check();
+    
+     bt.printf("\n\n");
+     bt.printf("\n\rMotor enabled ? : %d ", motor_enabled );
+     bt.printf("\n\rSteering enabled ? : %d ", steering_enabled );
+     bt.printf("\n\rCurrent duty cycle : %f", current_duty_cycle);
+     bt.printf("\n\Brake enabled ? : %d ", brakeLeftBool );
+     bt.printf("\n\rCurrent duty cycle? : %f ", current_duty_cycle );
       //wait(0.5); //commented out the wait bc it slows down the fault_check, and it breaks the analogIn readings for the driving input
                     //the driving input ticker is faster than the analog.read() function, so all analog.read() methods must be in the main loop
      switch(current_state ){
-         case STOP :    pc.printf("\r\nCurrent state is stop"); break; 
-         case RUN:      pc.printf("\r\nCurrent state is RUN"); break; 
-         case WAIT :    pc.printf("\r\nCurrent state is WAIT"); break; 
+         case STOP :    bt.printf("\r\nCurrent state is stop"); break; 
+         case RUN:      bt.printf("\r\nCurrent state is RUN"); break; 
+         case WAIT :    bt.printf("\r\nCurrent state is WAIT"); break; 
          };
      switch(fault_type) {
-         case CLEAR :       pc.printf("\r\nFault: CLEAR"); break; 
-         case OFF_TRACK:    pc.printf("\r\nFault: OFF TRACK"); break;
-         case BUMPER :      pc.printf("\r\nFault: OBSTACLE"); break;
-         case LOW_VOLTAGE : pc.printf("Fault: LOW VOLTAGE");  break;
+         case CLEAR :       bt.printf("\r\nFault: CLEAR"); break; 
+         case OFF_TRACK:    bt.printf("\r\nFault: OFF TRACK"); break;
+         case BUMPER :      bt.printf("\r\nFault: OBSTACLE"); break;
+         case LOW_VOLTAGE : bt.printf("\r\nFault: LOW VOLTAGE");  break;
           }
           
 x = abs(acc.getAccX());
@@ -88,7 +94,7 @@
 
 speedLeftVolt = (speedSensorLeft * 3.3f);
 speedRightVolt = (speedSensorRight * 3.3f);
-
+/*
 bt.printf("Duty Cycle = %1.2f     ", dutyCycleLeft);
 bt.printf("Speed (V) L,R = %1.2f", speedLeftVolt);
 bt.printf(", %1.2f     ", speedRightVolt);
@@ -100,11 +106,11 @@
 bt.printf("Output: %1.2f     ", controllerOutputLeft);
 
 bt.printf("Batt Voltage: %1.2f \n",avgCellVoltage);
-
+*/
 //setpointLeft = pot1;
 //setpointRight = pot1;
 
-
+/*
 if (t.read() > 5){
     setpointLeft = 0.0;
     setpointRight = 0.0;
@@ -114,6 +120,7 @@
         setpointLeft = 0.2;
         setpointRight = 0.2;
         }
+        */
 
  if (newData){  
       newData = false;