Simulation of flight pitch control with servo sweep.

Dependencies:   mbed Servo mbed-rtos Motor

Committer:
dreschpe
Date:
Wed Dec 05 08:06:20 2012 +0000
Revision:
3:3ec443c0842a
Parent:
2:a69c8c5f5b03
Child:
4:79863d2ea5a0
the lcd lib is not claiming the printf now, so we have to call LCD.printf

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 // Pot1 change the contrast
dreschpe 1:1c6a9eaf55b5 3 // Pot2 change the speed of the sin wave
dreschpe 1:1c6a9eaf55b5 4
dreschpe 0:f6a57b843f79 5 #include "mbed.h"
dreschpe 0:f6a57b843f79 6 #include "rtos.h"
dreschpe 0:f6a57b843f79 7 #include "Small_6.h"
dreschpe 0:f6a57b843f79 8 #include "Small_7.h"
dreschpe 0:f6a57b843f79 9 #include "Arial_9.h"
dreschpe 0:f6a57b843f79 10 #include "stdio.h"
dreschpe 1:1c6a9eaf55b5 11 #include "C12832_lcd.h"
dreschpe 0:f6a57b843f79 12
dreschpe 0:f6a57b843f79 13 // LCD object
dreschpe 3:3ec443c0842a 14 C12832_LCD LCD;
dreschpe 0:f6a57b843f79 15
dreschpe 1:1c6a9eaf55b5 16 AnalogIn Pot1(p19);
dreschpe 1:1c6a9eaf55b5 17 AnalogIn Pot2(p20);
dreschpe 0:f6a57b843f79 18
dreschpe 2:a69c8c5f5b03 19
dreschpe 2:a69c8c5f5b03 20 // mutex to make the lcd lib thread save
dreschpe 1:1c6a9eaf55b5 21 Mutex lcd_mutex;
dreschpe 0:f6a57b843f79 22
dreschpe 2:a69c8c5f5b03 23 // Thread 1
dreschpe 2:a69c8c5f5b03 24 // print counter into first line and wait for 1 s
dreschpe 0:f6a57b843f79 25 void thread1(void const *args)
dreschpe 0:f6a57b843f79 26 {
dreschpe 0:f6a57b843f79 27 int i;
dreschpe 0:f6a57b843f79 28 while(true) { // thread loop
dreschpe 2:a69c8c5f5b03 29 lcd_mutex.lock();
dreschpe 0:f6a57b843f79 30 LCD.locate(0,0);
dreschpe 0:f6a57b843f79 31 LCD.set_font((unsigned char*) Small_6);
dreschpe 3:3ec443c0842a 32 LCD.printf("Thread1 count: %d",i);
dreschpe 2:a69c8c5f5b03 33 lcd_mutex.unlock();
dreschpe 0:f6a57b843f79 34 i++;
dreschpe 0:f6a57b843f79 35 Thread::wait(1000);
dreschpe 0:f6a57b843f79 36 }
dreschpe 0:f6a57b843f79 37 }
dreschpe 0:f6a57b843f79 38
dreschpe 2:a69c8c5f5b03 39 // Thread 2
dreschpe 2:a69c8c5f5b03 40 // print counter into third line and wait for 0,5s
dreschpe 0:f6a57b843f79 41 void thread2(void const *args)
dreschpe 0:f6a57b843f79 42 {
dreschpe 1:1c6a9eaf55b5 43 int k;
dreschpe 0:f6a57b843f79 44 while(true) { // thread loop
dreschpe 2:a69c8c5f5b03 45 lcd_mutex.lock();
dreschpe 0:f6a57b843f79 46 LCD.locate(0,20);
dreschpe 0:f6a57b843f79 47 LCD.set_font((unsigned char*) Arial_9);
dreschpe 3:3ec443c0842a 48 LCD.printf("Thread 2 count : %d",k);
dreschpe 2:a69c8c5f5b03 49 lcd_mutex.unlock();
dreschpe 1:1c6a9eaf55b5 50 k++;
dreschpe 2:a69c8c5f5b03 51 Thread::wait(500); // wait 0.5s
dreschpe 0:f6a57b843f79 52 }
dreschpe 0:f6a57b843f79 53 }
dreschpe 0:f6a57b843f79 54
dreschpe 2:a69c8c5f5b03 55 // Thread 3
dreschpe 2:a69c8c5f5b03 56 // print a sin function in a small window
dreschpe 2:a69c8c5f5b03 57 // the value of pot 1 change the speed of the sinwave
dreschpe 2:a69c8c5f5b03 58 void thread3(void const *args)
dreschpe 1:1c6a9eaf55b5 59 {
dreschpe 1:1c6a9eaf55b5 60 int i,k,v;
dreschpe 1:1c6a9eaf55b5 61 double s,a;
dreschpe 1:1c6a9eaf55b5 62 k = 1;
dreschpe 2:a69c8c5f5b03 63 lcd_mutex.lock();
dreschpe 1:1c6a9eaf55b5 64 LCD.rect(89,0,127,17,1);
dreschpe 2:a69c8c5f5b03 65 lcd_mutex.unlock();
dreschpe 1:1c6a9eaf55b5 66 while(true) { // thread loop
dreschpe 1:1c6a9eaf55b5 67 v = Pot1.read_u16(); // get value of pot 1
dreschpe 2:a69c8c5f5b03 68 lcd_mutex.lock();
dreschpe 1:1c6a9eaf55b5 69 for (i=90; i<127; i++) {
dreschpe 2:a69c8c5f5b03 70 s = 8 * sin((long double)(i+k) /5); // pixel to print
dreschpe 2:a69c8c5f5b03 71 a = 8 * sin((long double)(i+k-1) /5); // old pixel to erase
dreschpe 2:a69c8c5f5b03 72 LCD.pixel(i,9 + (int)a ,0); // erase pixel
dreschpe 2:a69c8c5f5b03 73 LCD.pixel(i,9 + (int)s ,1); // print pixel
dreschpe 1:1c6a9eaf55b5 74 }
dreschpe 2:a69c8c5f5b03 75 LCD.copy_to_lcd(); // LCD.pixel do not update the lcd
dreschpe 2:a69c8c5f5b03 76 lcd_mutex.unlock();
dreschpe 1:1c6a9eaf55b5 77 k++;
dreschpe 2:a69c8c5f5b03 78 Thread::wait(v/100); // value of pot1 / 100
dreschpe 1:1c6a9eaf55b5 79 }
dreschpe 1:1c6a9eaf55b5 80 }
dreschpe 0:f6a57b843f79 81
dreschpe 2:a69c8c5f5b03 82 // Thread 4
dreschpe 2:a69c8c5f5b03 83 // input pot 2 and change the contrast of lcd
dreschpe 2:a69c8c5f5b03 84 void thread4(void const *args)
dreschpe 1:1c6a9eaf55b5 85 {
dreschpe 1:1c6a9eaf55b5 86 int k;
dreschpe 1:1c6a9eaf55b5 87 while(true) { // thread loop
dreschpe 1:1c6a9eaf55b5 88 k = Pot2.read_u16(); // get the value of poti 2
dreschpe 2:a69c8c5f5b03 89 k = k >> 10; // we need only 6 bit for contrast
dreschpe 2:a69c8c5f5b03 90 lcd_mutex.lock();
dreschpe 1:1c6a9eaf55b5 91 LCD.set_contrast(k);
dreschpe 2:a69c8c5f5b03 92 lcd_mutex.unlock();
dreschpe 2:a69c8c5f5b03 93 Thread::wait(500); // wait 0.5s
dreschpe 1:1c6a9eaf55b5 94 }
dreschpe 1:1c6a9eaf55b5 95 }
dreschpe 1:1c6a9eaf55b5 96
dreschpe 2:a69c8c5f5b03 97
dreschpe 2:a69c8c5f5b03 98 // print the actual contrast
dreschpe 0:f6a57b843f79 99 int main()
dreschpe 0:f6a57b843f79 100 {
dreschpe 0:f6a57b843f79 101 int j;
dreschpe 2:a69c8c5f5b03 102 LCD.cls();
dreschpe 2:a69c8c5f5b03 103
dreschpe 2:a69c8c5f5b03 104 Thread t1(thread1); //start thread1
dreschpe 2:a69c8c5f5b03 105 Thread t2(thread2); //start thread2
dreschpe 2:a69c8c5f5b03 106 Thread t3(thread3); //start thread3
dreschpe 2:a69c8c5f5b03 107 Thread t4(thread4); //start thread4
dreschpe 0:f6a57b843f79 108
dreschpe 2:a69c8c5f5b03 109 while(true) { // main is the next thread
dreschpe 2:a69c8c5f5b03 110 lcd_mutex.lock();
dreschpe 1:1c6a9eaf55b5 111 LCD.locate(0,9);
dreschpe 0:f6a57b843f79 112 LCD.set_font((unsigned char*) Small_7);
dreschpe 2:a69c8c5f5b03 113 j = LCD.get_contrast(); // read the actual contrast
dreschpe 3:3ec443c0842a 114 LCD.printf("contrast : %d",j);
dreschpe 2:a69c8c5f5b03 115 lcd_mutex.unlock();
dreschpe 2:a69c8c5f5b03 116 Thread::wait(500); // wait 0.5s
dreschpe 0:f6a57b843f79 117 }
dreschpe 1:1c6a9eaf55b5 118
dreschpe 0:f6a57b843f79 119 }