Karim Azzouz
A quadcopter control Software (Still in development). achieved single axis stability!!!!! released for others benefit. if you'd like to help co-develop this code, then please let me know
Dependencies: MovingAverageFilter MyI2C PID RC mbed-rtos mbed
Currently on hold, due to the fact that i don't own a RX/TX system
Diff: Motors.h
- Revision:
- 0:54b67cd15a5b
- Child:
- 1:e08a4f517989
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motors.h Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,57 @@
+#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){
+
+ 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);
+
+ 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){
+
+ 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;
+
+ D[PITCH] = Scaled_GyroX;
+ D[ROLL] = Scaled_GyroY;
+ D[YAW] = Scaled_GyroZ;
+
+ 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));
+}