main.cpp

00001 #include "Rover.h"
00002 #include "PwmIn.h"
00003 
00004 DigitalOut ledOrange(LED_ORANGE);
00005 DigitalOut ledRed(LED_RED);
00006 DigitalOut heartbeat(LOOP_HB);
00007 
00008 //PWM object
00009 PwmIn PWM_longitudinal(SERVO_1);
00010 PwmIn PWM_onoff(SERVO_2);
00011 
00012 float readVelPwm(float minPwm, float maxPwm, float zeroPwm, float deadzone, float maxVel){
00013     float in = 0.0;
00014     float norm = 0.0;
00015     in = PWM_longitudinal.pulsewidth()*1e6; //s to us
00016     //printf("pwm_vel: %f \r\n", in);
00017     // check it's within the range
00018     if(in < minPwm) in = minPwm;
00019     if(in > maxPwm) in = maxPwm;
00020     if(in < zeroPwm+deadzone && in > zeroPwm-deadzone) in = zeroPwm;   
00021     // normalize range
00022     norm = (in-minPwm)/(maxPwm-minPwm);
00023     // scale to +- maxVel
00024     return maxVel*(norm*2.0-1.0);
00025 }
00026 
00027 int readSwitchPwm(float downLo, float downHi, float centerLo, float centerHi, float upLo, float upHi){
00028     float in = 0.0;
00029     int switchState = 0.0;
00030     in = PWM_onoff.pulsewidth()*1e6; //s to us
00031     //printf("pwm_onoff: %f \r\n", in);
00032     // check it's within the range
00033     if( in > downLo && in < downHi) switchState = 1;
00034     if( in > centerLo && in < centerHi) switchState = 2;
00035     if( in > upLo && in < upHi) switchState = 3;
00036     
00037     return switchState;
00038 }
00039 
00040 ///main loop
00041 int main() {
00042     float dtBoard = dtBoard_;
00043     float actualTime = 0.00f;
00044     float precTime = 0.00f;
00045     Timer timeCheckTimer;
00046     
00047     float vels_d  = 0.00f;
00048     float velf_d  = 0.00f; //vel avanzamento
00049     float pitch_d = 0.00f;
00050     float pitch_m = 0.00f;
00051     float pitch_vel_m = 0.00f;
00052     float roll_m = 0.00f;
00053     int swState = 0;
00054     
00055     Rover* rover;
00056     
00057     rover = new Rover();     
00058     rover->setRoverKin(r_wheels_, longitudinalSemiDist_, lateralSemiDist_);
00059     rover->initializeImu();
00060     
00061     timeCheckTimer.start();    
00062     ThisThread::sleep_for(1000); 
00063     //           fw_l_1, fw_r_2, bw_l_3, bw_r_4
00064     //rover->setMotorsVelocity(-1.0, 1.0, 1.0, -1.0, wheelsAcceleration_);
00065 
00066     while (1){
00067         
00068         //control.stabilizationFunction();
00069         
00070         velf_d = readVelPwm( minPwmVel_, maxPwmVel_, zeroPwmVel_, deadzoneVel_, maxVel_);
00071         swState = readSwitchPwm(downLo_, downHi_, centerLo_, centerHi_, upLo_, upHi_);
00072         //printf("state: %d \r\n", swState);
00073         //printf("vel: %f \r\n", velf_d);
00074         
00075         if(swState ==0){ //no signal or somwthing strange
00076             rover->setRoverVelocity(0.0, 0.0 , 0.0, wheelsAcceleration_);
00077             ledOrange =0;
00078             ledRed = 1;
00079         }
00080         
00081         if(swState ==3){ //cruise control
00082             velf_d = constTravelVel_;
00083         }
00084         
00085         if(swState > 1){
00086             ledOrange = !ledOrange;
00087             ledRed = 0;
00088             rover->calcImuAngles(pitch_m, pitch_vel_m, roll_m, dtBoard);
00089             //std::cout  << "pd: " << pitch_d << "  pm: " << pitch_d << std::endl;
00090             //printf("pd: %f pm: %f \n",pitch_d,pitch_m);
00091             vels_d = rover->computeStabilizationVel(pitch_d, pitch_m, pitch_vel_m, Kp_stabilization_, Kd_stabilization_);
00092             rover->setRoverVelocity(velf_d, vels_d , 0.0, wheelsAcceleration_); //velForward, velStabilization, velAsset, pipeRadious, acceleration
00093         }
00094         else{
00095             rover->setRoverVelocity(0.0, 0.0 , 0.0, wheelsAcceleration_);
00096             ledOrange =0;
00097             ledRed = 1;
00098         }
00099         
00100         heartbeat = !heartbeat;//SCOPE for rate debug
00101         //ThisThread::sleep_for(2); 
00102         
00103         actualTime = timeCheckTimer.read();
00104         while(dtBoard > actualTime){
00105             ThisThread::sleep_for(1e-6);//0.01
00106             actualTime = timeCheckTimer.read();            
00107         }
00108         timeCheckTimer.reset();  
00109         
00110         
00111     }  
00112 
00113 }
00114