Simulation of flight pitch control with servo sweep.
Dependencies: mbed Servo mbed-rtos Motor
main.cpp
- Committer:
- alevale32
- Date:
- 2018-12-13
- Revision:
- 5:8db82a61e052
- Parent:
- 4:79863d2ea5a0
File content as of revision 5:8db82a61e052:
// example to test the mbed Lab Board lcd lib with the mbed rtos #include "mbed.h" #include "rtos.h" #include "stdio.h" #include "Motor.h" #include "LSM9DS1.h" #include "Servo.h" #define PI 3.14159 Serial pc(USBTX, USBRX); Motor m(p21, p16, p15); // pwm, fwd, rev Servo myservo(p24); // pwm using namespace std; // mutex to make pitch R/W safe Mutex pitch_mutex; // global pitch float value float pitch; // returns pitch, can also return roll float getAttitude(float ax, float ay, float az) { float roll = atan2(ay, az); float pitch_res = atan2(-ax, sqrt(ay * ay + az * az)); // Convert everything from radians to degrees: pitch_res *= 180.0 / PI; roll *= 180.0 / PI; if (pitch_res < -3.0 || pitch_res > 3.0) pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch_res,roll); return pitch_res; } // Thread 1 // propeller PWM control void thread1(void const *args) { while(true) { // thread loop pitch_mutex.lock(); // cruising speed between +/-15.0 degrees if (pitch < -15.0) m.speed(0.5 - ((pitch/-75.0 > 1.0 ? 1.0 : pitch/-75.0) * 0.5)); else if (pitch > 15.0) m.speed(0.5 + ((pitch/45.0 > 1.0 ? 1.0 : pitch/45.0) * 0.5)); else m.speed(0.5); pitch_mutex.unlock(); Thread::wait(100); // wait 0.1s } } // Thread 2 // Servo control void thread2(void const *args) { while(true) { // thread loop for (float pos = 0.0; pos <= 1.0; pos+=0.001){ myservo = pos; Thread::wait(10); // wait 0.01s } for (float pos = 1.0; pos >= 0.0; pos-=0.001){ myservo = pos; Thread::wait(10); // wait 0.01s } Thread::wait(500); // wait 0.5s } } int main() { myservo.calibrate(0.0011, 45.0); LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); IMU.begin(); if (!IMU.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } IMU.calibrate(1); while(!IMU.accelAvailable()); IMU.readAccel(); pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); float pitch_reading; Thread t1(thread1); //start thread1 Thread t2(thread2); //start thread2 // IMU reading control loop while(true){ while(!IMU.accelAvailable()); IMU.readAccel(); pitch_reading = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); pitch_mutex.lock(); pitch = pitch_reading; pitch_mutex.unlock(); Thread::wait(50); // wait 0.05s } }