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