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