Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
Diff: main.cpp
- Revision:
- 6:584653235830
- Parent:
- 4:3f22193053d0
- Child:
- 10:62e9b61ed1ad
diff -r 362759e42070 -r 584653235830 main.cpp --- 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); - - - - } + } }