Diff: main.cpp

Revision:

6:584653235830

Parent:

4:3f22193053d0

Child:

10:62e9b61ed1ad

--- a/main.cpp	Wed Jan 08 11:12:51 2020 +0000
+++ b/main.cpp	Tue Sep 14 11:18:35 2021 +0000
@@ -1,160 +1,50 @@
-#include "mbed.h"
-#include "Servo.h"
-#include "Rover.h"
-#include "math.h"
-#include "TOFs.h"
-//#include "MX.h"
-//#include "UARTSerial_half.h"
+#include "systemControl.h"
 
-#define DEBUG true
-#define READ true 
-
- 
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 
-//UARTSerial_half dxl_port(PB_9,PB_8,PF_13, 1000000); 
-
-//int ID[5] = {1,2,3,4,5};
-//MX mx_MotorChain(ID, sizeof(ID)/sizeof(int), 1000000, &dxl_port);  
-
 int main() {
-      
-      
- /*           bool enableVal[5] = {1,1,1,1,1};
-            mx_MotorChain.SyncTorqueEnable(enableVal);
-            
-            float Goal_position[5] = {100,20,150,60,75};
-            mx_MotorChain.SyncSetGoal(Goal_position); */
-      
-    float velf_a;
-    float vels_a;    
-    float vela_a; 
-    float velf_m = 0.0;
-    float vels_m = 0.0;    
-    float vela_m = 0.0;
     
-    float front_vel_dx = 0.0;
-    float front_vel_sx = 0.0;    
-    float retro_vel_dx = 0.0;
-    float retro_vel_sx = 0.0;
-    float forward_vel = 0.0; 
-    float pitch;
-    
-    float jointFront_d = 0.0;
-    float jointRetro_d = 0.0;
-    float time = 0.0;
+    SystemControl control;
+    Timer timeCheckTimer;
     
-     
-    float roll;
-    printf("Creo il rover \r\n");
-    Rover rover;
-    
-    
-    rover.initializeImu();
-    rover.initializeTofs(); 
-
-    
-    Timer schedulerTimer;  
-
-    schedulerTimer.start();     
-
-    rover.setCentralJointsAngle(0.0,0.0);  //in radiants  (front, retro)
-    rover.setWheelsVelocity(0.0,0.0 , 0, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
-    
-    rover.startEthComunication();
-
-    wait(1);
-    
-    double timeStabPrec = 0.0;
-    double timeReadPrec = 0.0;   
-    double timePrintPrec = 0.0;    
-
-    timeStabPrec = schedulerTimer.read();
-    timePrintPrec = schedulerTimer.read();    
-    timeReadPrec = schedulerTimer.read();    
-
-    float dtStab = 5.0/1000;
-    float dtRead = 20.0/1000;   
-    float dtPrint = 500.0/1000;
-    float dtPassedStab = 0.0;
-    float Kp=1.2; //0.6
-    
-    float pitch_d = 0.0;
+    float actualTime = 0.0;
+    float precTime = 0.0;
+  
+    ThisThread::sleep_for(2000); 
     
-    float frontDistance;
-    float retroDistance;   
-        
-    while (1){
-         
-         time = schedulerTimer.read();
-         
-
-         if(time - timeStabPrec > dtStab){  
-         
-             //rover.acquireTofs(frontDistance, retroDistance); 
-             
-             rover.computeCentralJointsFromTofs();
-             dtPassedStab = time - timeStabPrec; 
-             timeStabPrec = time;                
-
-             rover.calcImuAngles(pitch, roll, dtStab);
-
-             velf_a = forward_vel;
-             vels_a = -Kp*M_PI*(pitch_d-pitch)/180;
-             vela_a = 0.0;
-                          
-             rover.setWheelsVelocity(velf_a, vels_a , vela_a, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
-             //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); 
-             
-             forward_vel = (float)rover.get_forward_vel()/1000;
-             pitch_d = (float)rover.get_pitch()/1000;
-             
-             jointFront_d = (float)rover.get_jointFront()/1000;
-             jointRetro_d = (float)rover.get_jointRetro()/1000;
-             
-             //rover.setCentralJointsAngle(jointFront_d,jointRetro_d);  //in radiants  (front, retro)
+    control.initEthernet();
+    control.initArm();
+    control.initRover();
+    timeCheckTimer.start();    
+    ThisThread::sleep_for(1000); 
 
-             
-         }
-         
-          if(READ && time - timeReadPrec > dtRead){
-             
-             timeReadPrec = time;             
-             //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); 
-             rover.getRoverWheelsVelocity(front_vel_dx, front_vel_sx, retro_vel_dx, retro_vel_sx); 
-             
+    while (1){
+        
+                
+        control.updateEthCommunication();
+        control.armRoverKinematicControlFunction();
+        control.stabilizationFunction();
+        control.navigationFunction();
+        control.calcDroneDynamics();
+        
+        control.toolControlFunction();
+        
+        control.printThreadFunction();
+        
+        actualTime = timeCheckTimer.read();
+        control.actualDtControlCycle = actualTime;
+        
+        while(control.dtBoard > control.actualDtControlCycle){
+            ThisThread::sleep_for(0.01);
+            actualTime = timeCheckTimer.read();
+            control.actualDtControlCycle = actualTime;
+            
+        }
+        
+        timeCheckTimer.reset();  
  
-         }
-
-       
-         if(DEBUG && time - timePrintPrec > dtPrint){
-             timePrintPrec = time;
-              
-             printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance);
-               
-             //printf(" Ts: %4.2f \t Vfa: %4.4f \t Vfm %4.4f \t Vsa: %4.4f \t Vsm: %4.4f \t A: %4.4f \r\n",dtPassedStab*1000, velf_a, velf_m, vels_a, vels_m, amplitude);
-
-             if(rover.getEthState() == 1){ 
-                myled1 = true;
-                myled2 = false;
-             }else if(rover.getEthState() == 2){
-                myled1 = true;
-                myled2 = true;
-             }else{
-                myled1 = false;
-                myled2 = false;
-             }        
-                    
-             
-
-         }
-          rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, velf_a);
-          //rover.ethComunicationUpdate(schedulerTimer.read(), pitch, velf_m, vels_m, dtPassedStab*1000, vel_r);
-
-             
-        
-    }       
+    }      
 
 }