Simulation of flight pitch control with servo sweep.
Dependencies: mbed Servo mbed-rtos Motor
main.cpp@5:8db82a61e052, 2018-12-13 (annotated)
- Committer:
- alevale32
- Date:
- Thu Dec 13 22:02:40 2018 +0000
- Revision:
- 5:8db82a61e052
- Parent:
- 4:79863d2ea5a0
RTOS pitch control simulation with servo sweep
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dreschpe | 2:a69c8c5f5b03 | 1 | // example to test the mbed Lab Board lcd lib with the mbed rtos |
dreschpe | 1:1c6a9eaf55b5 | 2 | |
dreschpe | 0:f6a57b843f79 | 3 | #include "mbed.h" |
dreschpe | 0:f6a57b843f79 | 4 | #include "rtos.h" |
dreschpe | 0:f6a57b843f79 | 5 | #include "stdio.h" |
alevale32 | 5:8db82a61e052 | 6 | #include "Motor.h" |
alevale32 | 5:8db82a61e052 | 7 | #include "LSM9DS1.h" |
alevale32 | 5:8db82a61e052 | 8 | #include "Servo.h" |
alevale32 | 5:8db82a61e052 | 9 | |
alevale32 | 5:8db82a61e052 | 10 | #define PI 3.14159 |
dreschpe | 0:f6a57b843f79 | 11 | |
alevale32 | 5:8db82a61e052 | 12 | Serial pc(USBTX, USBRX); |
alevale32 | 5:8db82a61e052 | 13 | Motor m(p21, p16, p15); // pwm, fwd, rev |
alevale32 | 5:8db82a61e052 | 14 | Servo myservo(p24); // pwm |
alevale32 | 5:8db82a61e052 | 15 | |
alevale32 | 5:8db82a61e052 | 16 | using namespace std; |
alevale32 | 5:8db82a61e052 | 17 | |
alevale32 | 5:8db82a61e052 | 18 | // mutex to make pitch R/W safe |
alevale32 | 5:8db82a61e052 | 19 | Mutex pitch_mutex; |
4180_1 | 4:79863d2ea5a0 | 20 | |
alevale32 | 5:8db82a61e052 | 21 | // global pitch float value |
alevale32 | 5:8db82a61e052 | 22 | float pitch; |
dreschpe | 2:a69c8c5f5b03 | 23 | |
alevale32 | 5:8db82a61e052 | 24 | // returns pitch, can also return roll |
alevale32 | 5:8db82a61e052 | 25 | float getAttitude(float ax, float ay, float az) |
alevale32 | 5:8db82a61e052 | 26 | { |
alevale32 | 5:8db82a61e052 | 27 | float roll = atan2(ay, az); |
alevale32 | 5:8db82a61e052 | 28 | float pitch_res = atan2(-ax, sqrt(ay * ay + az * az)); |
alevale32 | 5:8db82a61e052 | 29 | |
alevale32 | 5:8db82a61e052 | 30 | // Convert everything from radians to degrees: |
alevale32 | 5:8db82a61e052 | 31 | pitch_res *= 180.0 / PI; |
alevale32 | 5:8db82a61e052 | 32 | roll *= 180.0 / PI; |
alevale32 | 5:8db82a61e052 | 33 | if (pitch_res < -3.0 || pitch_res > 3.0) pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch_res,roll); |
alevale32 | 5:8db82a61e052 | 34 | return pitch_res; |
alevale32 | 5:8db82a61e052 | 35 | } |
dreschpe | 0:f6a57b843f79 | 36 | |
dreschpe | 2:a69c8c5f5b03 | 37 | // Thread 1 |
alevale32 | 5:8db82a61e052 | 38 | // propeller PWM control |
dreschpe | 0:f6a57b843f79 | 39 | void thread1(void const *args) |
dreschpe | 0:f6a57b843f79 | 40 | { |
alevale32 | 5:8db82a61e052 | 41 | |
dreschpe | 0:f6a57b843f79 | 42 | while(true) { // thread loop |
alevale32 | 5:8db82a61e052 | 43 | pitch_mutex.lock(); // cruising speed between +/-15.0 degrees |
alevale32 | 5:8db82a61e052 | 44 | if (pitch < -15.0) m.speed(0.5 - ((pitch/-75.0 > 1.0 ? 1.0 : pitch/-75.0) * 0.5)); |
alevale32 | 5:8db82a61e052 | 45 | else if (pitch > 15.0) m.speed(0.5 + ((pitch/45.0 > 1.0 ? 1.0 : pitch/45.0) * 0.5)); |
alevale32 | 5:8db82a61e052 | 46 | else m.speed(0.5); |
alevale32 | 5:8db82a61e052 | 47 | pitch_mutex.unlock(); |
alevale32 | 5:8db82a61e052 | 48 | Thread::wait(100); // wait 0.1s |
dreschpe | 0:f6a57b843f79 | 49 | } |
dreschpe | 0:f6a57b843f79 | 50 | } |
dreschpe | 0:f6a57b843f79 | 51 | |
dreschpe | 2:a69c8c5f5b03 | 52 | // Thread 2 |
alevale32 | 5:8db82a61e052 | 53 | // Servo control |
dreschpe | 0:f6a57b843f79 | 54 | void thread2(void const *args) |
dreschpe | 0:f6a57b843f79 | 55 | { |
dreschpe | 0:f6a57b843f79 | 56 | while(true) { // thread loop |
alevale32 | 5:8db82a61e052 | 57 | for (float pos = 0.0; pos <= 1.0; pos+=0.001){ |
alevale32 | 5:8db82a61e052 | 58 | myservo = pos; |
alevale32 | 5:8db82a61e052 | 59 | Thread::wait(10); // wait 0.01s |
dreschpe | 1:1c6a9eaf55b5 | 60 | } |
alevale32 | 5:8db82a61e052 | 61 | for (float pos = 1.0; pos >= 0.0; pos-=0.001){ |
alevale32 | 5:8db82a61e052 | 62 | myservo = pos; |
alevale32 | 5:8db82a61e052 | 63 | Thread::wait(10); // wait 0.01s |
alevale32 | 5:8db82a61e052 | 64 | } |
alevale32 | 5:8db82a61e052 | 65 | Thread::wait(500); // wait 0.5s |
dreschpe | 1:1c6a9eaf55b5 | 66 | } |
dreschpe | 1:1c6a9eaf55b5 | 67 | } |
dreschpe | 0:f6a57b843f79 | 68 | |
alevale32 | 5:8db82a61e052 | 69 | int main() { |
alevale32 | 5:8db82a61e052 | 70 | myservo.calibrate(0.0011, 45.0); |
alevale32 | 5:8db82a61e052 | 71 | LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); |
alevale32 | 5:8db82a61e052 | 72 | IMU.begin(); |
alevale32 | 5:8db82a61e052 | 73 | if (!IMU.begin()) { |
alevale32 | 5:8db82a61e052 | 74 | pc.printf("Failed to communicate with LSM9DS1.\n"); |
4180_1 | 4:79863d2ea5a0 | 75 | } |
alevale32 | 5:8db82a61e052 | 76 | IMU.calibrate(1); |
alevale32 | 5:8db82a61e052 | 77 | while(!IMU.accelAvailable()); |
alevale32 | 5:8db82a61e052 | 78 | IMU.readAccel(); |
alevale32 | 5:8db82a61e052 | 79 | pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); |
alevale32 | 5:8db82a61e052 | 80 | float pitch_reading; |
alevale32 | 5:8db82a61e052 | 81 | Thread t1(thread1); //start thread1 |
alevale32 | 5:8db82a61e052 | 82 | Thread t2(thread2); //start thread2 |
alevale32 | 5:8db82a61e052 | 83 | // IMU reading control loop |
alevale32 | 5:8db82a61e052 | 84 | while(true){ |
alevale32 | 5:8db82a61e052 | 85 | while(!IMU.accelAvailable()); |
alevale32 | 5:8db82a61e052 | 86 | IMU.readAccel(); |
alevale32 | 5:8db82a61e052 | 87 | pitch_reading = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); |
alevale32 | 5:8db82a61e052 | 88 | pitch_mutex.lock(); |
alevale32 | 5:8db82a61e052 | 89 | pitch = pitch_reading; |
alevale32 | 5:8db82a61e052 | 90 | pitch_mutex.unlock(); |
alevale32 | 5:8db82a61e052 | 91 | Thread::wait(50); // wait 0.05s |
4180_1 | 4:79863d2ea5a0 | 92 | } |
4180_1 | 4:79863d2ea5a0 | 93 | } |