Diff: Motors.h

Revision:

1:e08a4f517989

Parent:

0:54b67cd15a5b

--- a/Motors.h	Sun Dec 23 23:50:21 2012 +0000
+++ b/Motors.h	Tue Aug 27 09:38:49 2013 +0000
@@ -1,57 +1,49 @@
-#define Kp 1
-#define Ki 1
-#define Kd 1
-
-#define PITCH 0
-#define ROLL 1
-#define YAW 2
-
-   int constrain(int a , int b, int c){
-   
+#define RefreshRate 2500 // in us :400Hz
+    
+   static uint16 Constrain(uint16 a , uint16 b, uint16 c){
    if(a<b) return b;
    else if(a>c)return c;
    else return a;
    }
-
-   void InitializeMotors(void){
-   
-    FrontMotor.period_ms(20);
-    RearMotor.period_ms(20);
-    LeftMotor.period_ms(20);
-    RightMotor.period_ms(20);
-   
+ 
+  void CalibrateESC(void){
+  
+    FrontMotor.period_us(RefreshRate); // setting one will by default set all other pins to the same period 
+    
+    /*FrontMotor.pulsewidth_us(2000);
+    RearMotor.pulsewidth_us(2000);
+    LeftMotor.pulsewidth_us(2000);
+    RightMotor.pulsewidth_us(2000);
+    
+    wait(2);  still in testing phase */ 
+    
     FrontMotor.pulsewidth_us(1000);
     RearMotor.pulsewidth_us(1000);
     LeftMotor.pulsewidth_us(1000);
     RightMotor.pulsewidth_us(1000);
     
-    wait(3); // let the hardware settle
- }
- 
-  void updatePID(void){ 
+    wait(2);
+  }
+   
+  void updateMotors(int16 PitchCommand,int16 RollCommand){
   
-    error[PITCH]= RX_Data[PITCH] - pitch;
-    error[ROLL]= RX_Data[ROLL] - roll ;
-    error[YAW]= RX_Data[YAW] - Scaled_GyroZ;
-    
-    I[PITCH]+= error[PITCH]*Ki;
-    I[ROLL]+= error[ROLL]*Ki;
-    I[YAW]+= error[YAW]*Ki;
+    if(Throttle>=1200){
+
+    Mspeed[0] = Constrain((Throttle+PitchCommand),1200,2000);
+    Mspeed[1] = Constrain((Throttle-PitchCommand),1200,2000);
+    Mspeed[2] = Constrain((Throttle+RollCommand),1200,2000);
+    Mspeed[3] = Constrain((Throttle-RollCommand),1200,2000);
+    }
     
-    D[PITCH] = Scaled_GyroX;
-    D[ROLL] = Scaled_GyroY;
-    D[YAW] = Scaled_GyroZ;
+    else{
+    Mspeed[0] = 1000;
+    Mspeed[1] = 1000;
+    Mspeed[2] = 1000;
+    Mspeed[3] = 1000;
+    }
     
-    PID[PITCH] = error[PITCH]*Kp + I[PITCH] + D[PITCH]*Kd ; // Pitch correction
-    PID[ROLL]  = error[ROLL]*Kp + I[ROLL] + D[ROLL]*Kd ;    // Roll correction
-    PID[YAW]   = error[YAW] ;                               // Yaw correction 
-  
-   }
-   
-  void updateMotors(void){
-  
-    FrontMotor.pulsewidth_us(constrain(RX_Data[3]+PID[PITCH],1000,2000));
-    RearMotor.pulsewidth_us(constrain(RX_Data[3]-PID[PITCH],1000,2000));
-    LeftMotor.pulsewidth_us(constrain(RX_Data[3]+PID[ROLL],1000,2000));
-    RightMotor.pulsewidth_us(constrain(RX_Data[3]-PID[ROLL],1000,2000));
+    FrontMotor.pulsewidth_us(Mspeed[0]);
+    RearMotor.pulsewidth_us(Mspeed[1]);
+    LeftMotor.pulsewidth_us(Mspeed[2]);
+    RightMotor.pulsewidth_us(Mspeed[3]);
 }