PRISMA Lab / Mbed OS Hyfliers_Completo_testato

Clone per testare Mbed studio

Dependencies:   PwmIn IONMcMotor MPU6050 Eigen ROVER

Fork of Hyfliers_Completo_testato by Marco De Silva

main.cpp@11:d93fbd8e02e9, 23 months ago (annotated)

Committer:
aledek
Date:
Wed Jul 27 09:37:11 2022 +0000
Revision:
11:d93fbd8e02e9
Parent:
10:62e9b61ed1ad 
first commit,

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aledek 10:62e9b61ed1ad 1 #include "Rover.h"
aledek 10:62e9b61ed1ad 2 #include "PwmIn.h"
aledek 10:62e9b61ed1ad 3
aledek 10:62e9b61ed1ad 4 DigitalOut ledOrange(LED_ORANGE);
aledek 10:62e9b61ed1ad 5 DigitalOut ledRed(LED_RED);
aledek 10:62e9b61ed1ad 6 DigitalOut heartbeat(LOOP_HB);
aledek 10:62e9b61ed1ad 7
aledek 10:62e9b61ed1ad 8 //PWM object
aledek 10:62e9b61ed1ad 9 PwmIn PWM_longitudinal(SERVO_1);
aledek 10:62e9b61ed1ad 10 PwmIn PWM_onoff(SERVO_2);
group-ST 0:6e8275981824 11
aledek 10:62e9b61ed1ad 12 float readVelPwm(float minPwm, float maxPwm, float zeroPwm, float deadzone, float maxVel){
aledek 10:62e9b61ed1ad 13 float in = 0.0;
aledek 10:62e9b61ed1ad 14 float norm = 0.0;
aledek 10:62e9b61ed1ad 15 in = PWM_longitudinal.pulsewidth()*1e6; //s to us
aledek 10:62e9b61ed1ad 16 //printf("pwm_vel: %f \r\n", in);
aledek 10:62e9b61ed1ad 17 // check it's within the range
aledek 10:62e9b61ed1ad 18 if(in < minPwm) in = minPwm;
aledek 10:62e9b61ed1ad 19 if(in > maxPwm) in = maxPwm;
aledek 10:62e9b61ed1ad 20 if(in < zeroPwm+deadzone && in > zeroPwm-deadzone) in = zeroPwm;
aledek 10:62e9b61ed1ad 21 // normalize range
aledek 10:62e9b61ed1ad 22 norm = (in-minPwm)/(maxPwm-minPwm);
aledek 10:62e9b61ed1ad 23 // scale to +- maxVel
aledek 10:62e9b61ed1ad 24 return maxVel*(norm*2.0-1.0);
aledek 10:62e9b61ed1ad 25 }
anfontanelli 3:fc26045926d9 26
aledek 10:62e9b61ed1ad 27 int readSwitchPwm(float downLo, float downHi, float centerLo, float centerHi, float upLo, float upHi){
aledek 10:62e9b61ed1ad 28 float in = 0.0;
aledek 10:62e9b61ed1ad 29 int switchState = 0.0;
aledek 10:62e9b61ed1ad 30 in = PWM_onoff.pulsewidth()*1e6; //s to us
aledek 10:62e9b61ed1ad 31 //printf("pwm_onoff: %f \r\n", in);
aledek 10:62e9b61ed1ad 32 // check it's within the range
aledek 10:62e9b61ed1ad 33 if( in > downLo && in < downHi) switchState = 1;
aledek 10:62e9b61ed1ad 34 if( in > centerLo && in < centerHi) switchState = 2;
aledek 10:62e9b61ed1ad 35 if( in > upLo && in < upHi) switchState = 3;
anfontanelli 3:fc26045926d9 36
aledek 10:62e9b61ed1ad 37 return switchState;
aledek 10:62e9b61ed1ad 38 }
aledek 10:62e9b61ed1ad 39
aledek 10:62e9b61ed1ad 40 ///main loop
aledek 10:62e9b61ed1ad 41 int main() {
aledek 10:62e9b61ed1ad 42 float dtBoard = dtBoard_;
aledek 10:62e9b61ed1ad 43 float actualTime = 0.00f;
aledek 10:62e9b61ed1ad 44 float precTime = 0.00f;
anfontanelli 6:584653235830 45 Timer timeCheckTimer;
anfontanelli 3:fc26045926d9 46
aledek 10:62e9b61ed1ad 47 float vels_d = 0.00f;
aledek 10:62e9b61ed1ad 48 float velf_d = 0.00f; //vel avanzamento
aledek 10:62e9b61ed1ad 49 float pitch_d = 0.00f;
aledek 10:62e9b61ed1ad 50 float pitch_m = 0.00f;
aledek 10:62e9b61ed1ad 51 float pitch_vel_m = 0.00f;
aledek 10:62e9b61ed1ad 52 float roll_m = 0.00f;
aledek 10:62e9b61ed1ad 53 int swState = 0;
group-ST 0:6e8275981824 54
aledek 10:62e9b61ed1ad 55 Rover* rover;
aledek 10:62e9b61ed1ad 56
aledek 10:62e9b61ed1ad 57 rover = new Rover();
aledek 10:62e9b61ed1ad 58 rover->setRoverKin(r_wheels_, longitudinalSemiDist_, lateralSemiDist_);
aledek 10:62e9b61ed1ad 59 rover->initializeImu();
aledek 10:62e9b61ed1ad 60
anfontanelli 6:584653235830 61 timeCheckTimer.start();
anfontanelli 6:584653235830 62 ThisThread::sleep_for(1000);
aledek 10:62e9b61ed1ad 63 // fw_l_1, fw_r_2, bw_l_3, bw_r_4
aledek 10:62e9b61ed1ad 64 //rover->setMotorsVelocity(-1.0, 1.0, 1.0, -1.0, wheelsAcceleration_);
anfontanelli 3:fc26045926d9 65
anfontanelli 6:584653235830 66 while (1){
anfontanelli 6:584653235830 67
aledek 10:62e9b61ed1ad 68 //control.stabilizationFunction();
aledek 10:62e9b61ed1ad 69
aledek 10:62e9b61ed1ad 70 velf_d = readVelPwm( minPwmVel_, maxPwmVel_, zeroPwmVel_, deadzoneVel_, maxVel_);
aledek 10:62e9b61ed1ad 71 swState = readSwitchPwm(downLo_, downHi_, centerLo_, centerHi_, upLo_, upHi_);
aledek 10:62e9b61ed1ad 72 //printf("state: %d \r\n", swState);
aledek 10:62e9b61ed1ad 73 //printf("vel: %f \r\n", velf_d);
aledek 10:62e9b61ed1ad 74
aledek 10:62e9b61ed1ad 75 if(swState ==0){ //no signal or somwthing strange
aledek 10:62e9b61ed1ad 76 rover->setRoverVelocity(0.0, 0.0 , 0.0, wheelsAcceleration_);
aledek 10:62e9b61ed1ad 77 ledOrange =0;
aledek 10:62e9b61ed1ad 78 ledRed = 1;
aledek 10:62e9b61ed1ad 79 }
aledek 10:62e9b61ed1ad 80
aledek 10:62e9b61ed1ad 81 if(swState ==3){ //cruise control
aledek 10:62e9b61ed1ad 82 velf_d = constTravelVel_;
aledek 10:62e9b61ed1ad 83 }
anfontanelli 6:584653235830 84
aledek 10:62e9b61ed1ad 85 if(swState > 1){
aledek 10:62e9b61ed1ad 86 ledOrange = !ledOrange;
aledek 10:62e9b61ed1ad 87 ledRed = 0;
aledek 10:62e9b61ed1ad 88 rover->calcImuAngles(pitch_m, pitch_vel_m, roll_m, dtBoard);
aledek 10:62e9b61ed1ad 89 //std::cout << "pd: " << pitch_d << " pm: " << pitch_d << std::endl;
aledek 10:62e9b61ed1ad 90 //printf("pd: %f pm: %f \n",pitch_d,pitch_m);
aledek 10:62e9b61ed1ad 91 vels_d = rover->computeStabilizationVel(pitch_d, pitch_m, pitch_vel_m, Kp_stabilization_, Kd_stabilization_);
aledek 10:62e9b61ed1ad 92 rover->setRoverVelocity(velf_d, vels_d , 0.0, wheelsAcceleration_); //velForward, velStabilization, velAsset, pipeRadious, acceleration
aledek 10:62e9b61ed1ad 93 }
aledek 10:62e9b61ed1ad 94 else{
aledek 10:62e9b61ed1ad 95 rover->setRoverVelocity(0.0, 0.0 , 0.0, wheelsAcceleration_);
aledek 10:62e9b61ed1ad 96 ledOrange =0;
aledek 10:62e9b61ed1ad 97 ledRed = 1;
aledek 10:62e9b61ed1ad 98 }
anfontanelli 6:584653235830 99
aledek 10:62e9b61ed1ad 100 heartbeat = !heartbeat;//SCOPE for rate debug
aledek 10:62e9b61ed1ad 101 //ThisThread::sleep_for(2);
anfontanelli 6:584653235830 102
anfontanelli 6:584653235830 103 actualTime = timeCheckTimer.read();
aledek 10:62e9b61ed1ad 104 while(dtBoard > actualTime){
aledek 10:62e9b61ed1ad 105 ThisThread::sleep_for(1e-6);//0.01
aledek 10:62e9b61ed1ad 106 actualTime = timeCheckTimer.read();
aledek 10:62e9b61ed1ad 107 }
aledek 10:62e9b61ed1ad 108 timeCheckTimer.reset();
anfontanelli 6:584653235830 109
anfontanelli 6:584653235830 110
aledek 10:62e9b61ed1ad 111 }
anfontanelli 3:fc26045926d9 112
group-ST 0:6e8275981824 113 }
anfontanelli 3:fc26045926d9 114