Motors.h

00001 #define RefreshRate 2500 // in us :400Hz
00002     
00003    static uint16 Constrain(uint16 a , uint16 b, uint16 c){
00004    if(a<b) return b;
00005    else if(a>c)return c;
00006    else return a;
00007    }
00008  
00009   void CalibrateESC(void){
00010   
00011     FrontMotor.period_us(RefreshRate); // setting one will by default set all other pins to the same period 
00012     
00013     /*FrontMotor.pulsewidth_us(2000);
00014     RearMotor.pulsewidth_us(2000);
00015     LeftMotor.pulsewidth_us(2000);
00016     RightMotor.pulsewidth_us(2000);
00017     
00018     wait(2);  still in testing phase */ 
00019     
00020     FrontMotor.pulsewidth_us(1000);
00021     RearMotor.pulsewidth_us(1000);
00022     LeftMotor.pulsewidth_us(1000);
00023     RightMotor.pulsewidth_us(1000);
00024     
00025     wait(2);
00026   }
00027    
00028   void updateMotors(int16 PitchCommand,int16 RollCommand){
00029   
00030     if(Throttle>=1200){
00031 
00032     Mspeed[0] = Constrain((Throttle+PitchCommand),1200,2000);
00033     Mspeed[1] = Constrain((Throttle-PitchCommand),1200,2000);
00034     Mspeed[2] = Constrain((Throttle+RollCommand),1200,2000);
00035     Mspeed[3] = Constrain((Throttle-RollCommand),1200,2000);
00036     }
00037     
00038     else{
00039     Mspeed[0] = 1000;
00040     Mspeed[1] = 1000;
00041     Mspeed[2] = 1000;
00042     Mspeed[3] = 1000;
00043     }
00044     
00045     FrontMotor.pulsewidth_us(Mspeed[0]);
00046     RearMotor.pulsewidth_us(Mspeed[1]);
00047     LeftMotor.pulsewidth_us(Mspeed[2]);
00048     RightMotor.pulsewidth_us(Mspeed[3]);
00049 }