Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
main.cpp
- Committer:
- aledek
- Date:
- 2022-07-27
- Revision:
- 11:d93fbd8e02e9
- Parent:
- 10:62e9b61ed1ad
File content as of revision 11:d93fbd8e02e9:
#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); 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 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; return switchState; } ///main loop int main() { float dtBoard = dtBoard_; float actualTime = 0.00f; float precTime = 0.00f; Timer timeCheckTimer; 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; 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.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_; } 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; } heartbeat = !heartbeat;//SCOPE for rate debug //ThisThread::sleep_for(2); actualTime = timeCheckTimer.read(); while(dtBoard > actualTime){ ThisThread::sleep_for(1e-6);//0.01 actualTime = timeCheckTimer.read(); } timeCheckTimer.reset(); } }