Diff: main.cpp

Revision:

10:62e9b61ed1ad

Parent:

6:584653235830

diff -r 03e93b86a922 -r 62e9b61ed1ad main.cpp
--- a/main.cpp	Mon Dec 20 10:30:08 2021 +0000
+++ b/main.cpp	Wed Jul 27 09:35:20 2022 +0000
@@ -1,50 +1,114 @@
-#include "systemControl.h"
+#include "Rover.h"
+#include "PwmIn.h"
+
+DigitalOut ledOrange(LED_ORANGE);
+DigitalOut ledRed(LED_RED);
+DigitalOut heartbeat(LOOP_HB);
+
+//PWM object
+PwmIn PWM_longitudinal(SERVO_1);
+PwmIn PWM_onoff(SERVO_2);
 
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
+float readVelPwm(float minPwm, float maxPwm, float zeroPwm, float deadzone, float maxVel){
+    float in = 0.0;
+    float norm = 0.0;
+    in = PWM_longitudinal.pulsewidth()*1e6; //s to us
+    //printf("pwm_vel: %f \r\n", in);
+    // check it's within the range
+    if(in < minPwm) in = minPwm;
+    if(in > maxPwm) in = maxPwm;
+    if(in < zeroPwm+deadzone && in > zeroPwm-deadzone) in = zeroPwm;   
+    // normalize range
+    norm = (in-minPwm)/(maxPwm-minPwm);
+    // scale to +- maxVel
+    return maxVel*(norm*2.0-1.0);
+}
 
-int main() {
+int readSwitchPwm(float downLo, float downHi, float centerLo, float centerHi, float upLo, float upHi){
+    float in = 0.0;
+    int switchState = 0.0;
+    in = PWM_onoff.pulsewidth()*1e6; //s to us
+    //printf("pwm_onoff: %f \r\n", in);
+    // check it's within the range
+    if( in > downLo && in < downHi) switchState = 1;
+    if( in > centerLo && in < centerHi) switchState = 2;
+    if( in > upLo && in < upHi) switchState = 3;
     
-    SystemControl control;
+    return switchState;
+}
+
+///main loop
+int main() {
+    float dtBoard = dtBoard_;
+    float actualTime = 0.00f;
+    float precTime = 0.00f;
     Timer timeCheckTimer;
     
-    float actualTime = 0.0;
-    float precTime = 0.0;
-  
-    ThisThread::sleep_for(2000); 
+    float vels_d  = 0.00f;
+    float velf_d  = 0.00f; //vel avanzamento
+    float pitch_d = 0.00f;
+    float pitch_m = 0.00f;
+    float pitch_vel_m = 0.00f;
+    float roll_m = 0.00f;
+    int swState = 0;
     
-    control.initEthernet();
-    control.initArm();
-    control.initRover();
+    Rover* rover;
+    
+    rover = new Rover();     
+    rover->setRoverKin(r_wheels_, longitudinalSemiDist_, lateralSemiDist_);
+    rover->initializeImu();
+    
     timeCheckTimer.start();    
     ThisThread::sleep_for(1000); 
+    //           fw_l_1, fw_r_2, bw_l_3, bw_r_4
+    //rover->setMotorsVelocity(-1.0, 1.0, 1.0, -1.0, wheelsAcceleration_);
 
     while (1){
         
-                
-        control.updateEthCommunication();
-        control.armRoverKinematicControlFunction();
-        control.stabilizationFunction();
-        control.navigationFunction();
-        control.calcDroneDynamics();
+        //control.stabilizationFunction();
+        
+        velf_d = readVelPwm( minPwmVel_, maxPwmVel_, zeroPwmVel_, deadzoneVel_, maxVel_);
+        swState = readSwitchPwm(downLo_, downHi_, centerLo_, centerHi_, upLo_, upHi_);
+        //printf("state: %d \r\n", swState);
+        //printf("vel: %f \r\n", velf_d);
+        
+        if(swState ==0){ //no signal or somwthing strange
+            rover->setRoverVelocity(0.0, 0.0 , 0.0, wheelsAcceleration_);
+            ledOrange =0;
+            ledRed = 1;
+        }
+        
+        if(swState ==3){ //cruise control
+            velf_d = constTravelVel_;
+        }
         
-        control.toolControlFunction();
+        if(swState > 1){
+            ledOrange = !ledOrange;
+            ledRed = 0;
+            rover->calcImuAngles(pitch_m, pitch_vel_m, roll_m, dtBoard);
+            //std::cout  << "pd: " << pitch_d << "  pm: " << pitch_d << std::endl;
+            //printf("pd: %f pm: %f \n",pitch_d,pitch_m);
+            vels_d = rover->computeStabilizationVel(pitch_d, pitch_m, pitch_vel_m, Kp_stabilization_, Kd_stabilization_);
+            rover->setRoverVelocity(velf_d, vels_d , 0.0, wheelsAcceleration_); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+        }
+        else{
+            rover->setRoverVelocity(0.0, 0.0 , 0.0, wheelsAcceleration_);
+            ledOrange =0;
+            ledRed = 1;
+        }
         
-        control.printThreadFunction();
+        heartbeat = !heartbeat;//SCOPE for rate debug
+        //ThisThread::sleep_for(2); 
         
         actualTime = timeCheckTimer.read();
-        control.actualDtControlCycle = actualTime;
+        while(dtBoard > actualTime){
+            ThisThread::sleep_for(1e-6);//0.01
+            actualTime = timeCheckTimer.read();            
+        }
+        timeCheckTimer.reset();  
         
-        while(control.dtBoard > control.actualDtControlCycle){
-            ThisThread::sleep_for(0.01);
-            actualTime = timeCheckTimer.read();
-            control.actualDtControlCycle = actualTime;
-            
-        }
         
-        timeCheckTimer.reset();  
- 
-    }      
+    }  
 
 }