Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
main.cpp@11:d93fbd8e02e9, 2022-07-27 (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?
User | Revision | Line number | New 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 |