Simulation of flight pitch control with servo sweep.
Dependencies: mbed Servo mbed-rtos Motor
Diff: main.cpp
- Revision:
- 5:8db82a61e052
- Parent:
- 4:79863d2ea5a0
--- a/main.cpp Sun Sep 22 17:57:46 2013 +0000 +++ b/main.cpp Thu Dec 13 22:02:40 2018 +0000 @@ -1,164 +1,93 @@ // example to test the mbed Lab Board lcd lib with the mbed rtos -// Pot1 changes the contrast -// Pot2 changes the speed of the sin wave #include "mbed.h" #include "rtos.h" -#include "Small_6.h" -#include "Small_7.h" -#include "Arial_9.h" #include "stdio.h" -#include "C12832_lcd.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; -C12832_LCD LCD; -AnalogIn Pot1(p19); -AnalogIn Pot2(p20); -PwmOut Speaker(p26); -PwmOut RGBLED_r(p23); -PwmOut RGBLED_g(p24); -PwmOut RGBLED_b(p25); -DigitalIn joyfire(p14); -BusIn joy(p15,p12,p13,p16); -BusOut leds(LED1,LED2,LED3,LED4); +// global pitch float value +float pitch; -// mutex to make the lcd lib thread safe -Mutex lcd_mutex; +// 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 -// print counter into first line and wait for 1 s +// propeller PWM control void thread1(void const *args) { - int i; + while(true) { // thread loop - lcd_mutex.lock(); - LCD.locate(0,0); - LCD.set_font((unsigned char*) Small_6); - LCD.printf("Thread1 count: %d",i); - lcd_mutex.unlock(); - i++; - Thread::wait(1000); + 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 -// print counter into third line and wait for 0,5s +// Servo control void thread2(void const *args) { - int k; while(true) { // thread loop - lcd_mutex.lock(); - LCD.locate(0,20); - LCD.set_font((unsigned char*) Arial_9); - LCD.printf("Thread 2 count : %d",k); - lcd_mutex.unlock(); - k++; - Thread::wait(500); // wait 0.5s - } -} - -// Thread 3 -// print a sin function in a small window -// the value of pot 1 changes the speed of the sine wave -void thread3(void const *args) -{ - int i,k,v; - double s,a; - k = 1; - lcd_mutex.lock(); - LCD.rect(89,0,127,17,1); - lcd_mutex.unlock(); - while(true) { // thread loop - v = Pot1.read_u16(); // get value of pot 1 - lcd_mutex.lock(); - for (i=90; i<127; i++) { - s = 8 * sin((long double)(i+k) /5); // pixel to print - a = 8 * sin((long double)(i+k-1) /5); // old pixel to erase - LCD.pixel(i,9 + (int)a ,0); // erase pixel - LCD.pixel(i,9 + (int)s ,1); // print pixel + for (float pos = 0.0; pos <= 1.0; pos+=0.001){ + myservo = pos; + Thread::wait(10); // wait 0.01s } - LCD.copy_to_lcd(); // LCD.pixel does not update the lcd - lcd_mutex.unlock(); - k++; - Thread::wait(v/100); // value of pot1 / 100 + 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 } } -// Thread 4 -// input pot 2 and change the contrast of LCD -void thread4(void const *args) -{ - int k; - while(true) { // thread loop - k = Pot2.read_u16(); // get the value of poti 2 - k = k >> 10; // need only 6 bits for contrast - lcd_mutex.lock(); - LCD.set_contrast(k); - lcd_mutex.unlock(); - 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"); } -} -// Thread 5 -// RGB LED -void thread5(void const *args) -{ - while(true) { // thread loop - RGBLED_r = 0.5 + (rand() % 11)/20.0; - RGBLED_g = 0.5 + (rand() % 11)/20.0; - RGBLED_b = 0.5 + (rand() % 11)/20.0; - Thread::wait(1667); // wait 1.5s - } -} -// Thread 6 -// Speaker -void thread6(void const *args) -{ - while(true) { // thread loop - Speaker.period(1.0/800.0); - Speaker = 0.01; - Thread::wait(1000); // wait 1.0s - Speaker.period(1.0/969.0); - Speaker = 0.01; - Thread::wait(1000); // wait 1.0s + 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 } } - -// Thread 7 -// Joystick controls onboard mbed LEDs -void thread7(void const *args) -{ - while(true) { // thread loop - if (joyfire) { - leds = 0xf; - } else { - leds = joy; - } - Thread::wait(200); // wait 0.25s - } -} - - - -int main() -{ - int j; - LCD.cls(); - - Thread t1(thread1); //start thread1 - Thread t2(thread2); //start thread2 - Thread t3(thread3); //start thread3 - Thread t4(thread4); //start thread4 - Thread t5(thread5); //start thread5 - Thread t6(thread6); //start thread6 - Thread t7(thread7); //start thread7 - - while(true) { // main is the next thread - lcd_mutex.lock(); - LCD.locate(0,9); - LCD.set_font((unsigned char*) Small_7); - j = LCD.get_contrast(); // read the actual contrast - LCD.printf("contrast : %d",j); - lcd_mutex.unlock(); - Thread::wait(500); // wait 0.5s - } -}