Alexander Valencia / Mbed 2 deprecated Final_Project-RTO-Simulation

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?

UserRevisionLine numberNew 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 }