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();  
        
        
    }  

}