Diff: main.cpp

Revision:

5:e8d4095d9c19

Parent:

4:76b3113c79ff

Child:

6:849caec97744

--- a/main.cpp	Sun Dec 14 09:17:01 2014 +0000
+++ b/main.cpp	Fri Jan 09 22:37:38 2015 +0000
@@ -6,7 +6,7 @@
  *  http://www.page.sannet.ne.jp/kenjia/index.html
  *  http://mbed.org/users/kenjiArai/
  *      Created: November  29th, 2014
- *      Revised: December  14th, 2014
+ *      Revised: January    9th, 2015
  *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
@@ -17,15 +17,16 @@
 
 //  Include ---------------------------------------------------------------------------------------
 #include    "mbed.h"
-#include    "rtos.h"
 #include    "L3GD20.h"
 #include    "LIS3DH.h"
+#include    "TextLCD.h"
 #include    "ST7565_SPI_LCD.h"
-#include    "PID.h"
-#include    "stepper.h"
 
 //  Definition ------------------------------------------------------------------------------------
 #define USE_COM         // use Communication with PC(UART)
+#define USE_I2C_LCD
+//#define USE_SPI_LCD
+#define USE_I2C_SENSOR
  
 // Com
 #ifdef  USE_COM
@@ -42,15 +43,6 @@
 #define READABLE(x)         {;}
 #endif
 
-#define TIMEBASE        12000
-#define FIXED_STEPS     100
-
-#define PI              3.1415926536
-#define RAD_TO_DEG      57.29578
-#define TIME_BASE_S     0.01
-#define TIME_BASE_MS    ( TIME_BASE_S * 1000)
-#define RATE            0.1
-
 //  Object ----------------------------------------------------------------------------------------
 // LED's
 DigitalOut LEDs[4] = {
@@ -60,7 +52,7 @@
 DigitalIn   USER_SWITCH[2] = {
     #if defined(TARGET_RZ_A1H)
     DigitalIn(P6_0),  DigitalIn(P6_1)
-    #elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)\
+    #elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F411RE)\
      || defined(TARGET_NUCLEO_L152RE)
     DigitalIn(PC_13),  DigitalIn(A0)
     #elif defined(TARGET_LPC1768)
@@ -69,8 +61,6 @@
     DigitalIn(PTA4),  DigitalIn(PTC6)
     #endif
 };
-// Rotor
-STEPPER     rotor(D5, D4, D3, D2);
 // com
 #ifdef USE_COM
 Serial      pcx(USBTX, USBRX);      // Communication with Host
@@ -80,62 +70,32 @@
 L3GX_GYRO   gyro(i2c, L3GD20_V_CHIP_ADDR, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS);
 // Acc
 LIS3DH      acc(i2c, LIS3DH_G_CHIP_ADDR, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G);
+#ifdef USE_I2C_LCD
+// LCD
+TextLCD_I2C_N lcd0(&i2c, 0x7c, TextLCD::LCD16x2);  // LCD(Akizuki AQM0802A)
+#endif
+#ifdef USE_SPI_LCD
 // SPI LCD
 SPI         spi_lcd(D11, D12, D13); // mosi, miso, sck
 ST7565      lcd1(spi_lcd, D8, D9, D7, ST7565::AQM1248A); // spi,reset,a0,ncs, LCD(Akizuki AQM1248A)
-// Kc, Ti, Td, interval
-PID         controller(1.0, 0.0, 0.0, RATE);
-// Mutex
-Mutex       i2c_mutex; 
+#endif
 
 //  RAM -------------------------------------------------------------------------------------------
-Queue<uint32_t, 2> queue0;
-Queue<uint32_t, 2> queue1;
 float fa[3];    // Acc  0:X, 1:Y, 2:Z
 float fg[3];    // Gyro 0:X, 1:Y, 2:Z
-float accXangle;    // Angle calculate using the accelerometer
-float gyroXangle;   // Angle calculate using the gyro
-float kalAngleX;    // Calculate the angle using a Kalman filter
-float stp;
-float angle;
-
-uint8_t pls_width[MT_SLOP_STEP] = {5, 4, 3, 2, 1, 1, 1, 1, 1, 1 };
-
-/* Mail */
-typedef struct {
-  float    voltage; /* AD result of measured voltage */
-  float    current; /* AD result of measured current */
-  uint32_t counter; /* A counter value               */
-} mail_t;
- 
-Mail<mail_t, 16> mail_box;
 
 uint8_t show_flag;
+uint32_t count;
 
 //  ROM / Constant data ---------------------------------------------------------------------------
 
 //  Function prototypes ---------------------------------------------------------------------------
 
 //  Function prototypes ---------------------------------------------------------------------------
-extern int mon( void);
-extern float kalmanCalculate(float newAngle, float newRate, int looptime);
 
 //-------------------------------------------------------------------------------------------------
 //  Control Program
 //-------------------------------------------------------------------------------------------------
-void send_thread (void const *args) {
-    uint32_t i = 0;
-    while (true) {
-        i++; // fake data update
-        mail_t *mail = mail_box.alloc();
-        mail->voltage = (i * 0.1) * 33; 
-        mail->current = (i * 0.1) * 11;
-        mail->counter = i;
-        mail_box.put(mail);
-        Thread::wait(1000);
-    }
-}
-
 void blink(void const *n) {
     LEDs[(int)n] = !LEDs[(int)n];
 }
@@ -146,70 +106,43 @@
     } else {                    return 0;}
 }
 
-// Monitor program
-void monitor(void const *args){
-    while (true){
-        mon();
-    }
-}
-
-// Interrupt routine
-void queue_isr0() {
-    queue0.put((uint32_t*)1);
-}
-
-void queue_isr1() {
-    queue1.put((uint32_t*)1);
+// Update sensor data
+void update_angle(void){
+#ifdef USE_I2C_SENSOR
+    // read acceleration data from sensor
+    acc.read_data(fa);
+    // read gyroscope data from sensor
+    gyro.read_data(fg);
+#else
+    fa[0] = fa[1] = fa[2] = 1.11f;
+    fg[0] = fg[1] = fg[2] = 1.11f;
+#endif
 }
 
 // Update sensor data
-void update_angle(void const *args){
-    while (true) {
-        osEvent evt = queue0.get();
-        // ---->lock
-        i2c_mutex.lock();
-        // read acceleration data from sensor
-        acc.read_data(fa);
-        // read gyroscope data from sensor
-        gyro.read_data(fg);
-        // <----unlock
-        i2c_mutex.unlock();
-        // Calculate angle (degree)
-        accXangle = (atan2(-fa[1],fa[2])+PI)*RAD_TO_DEG;
-        // calculate filtered Angle
-        kalAngleX = kalmanCalculate(accXangle, fg[0], TIME_BASE_MS) - 180;
-    }
+void display(void){
+#ifdef USE_I2C_LCD
+    lcd0.locate(0, 0);    // 1st line top
+    lcd0.printf(" G=%4.1f ", sqrt(fa[0]*fa[0] + fa[1]*fa[1] + fa[2]*fa[2]));
+    lcd0.locate(0, 1);    // 2nd line top
+    lcd0.printf("%8d",count++);
+#endif
+#ifdef USE_SPI_LCD
+    lcd1.locate(0,0);
+    lcd1.printf("G:%+6.1f,%+6.1f,%+6.1f \r\n", fg[0], fg[1], fg[2]);
+    lcd1.printf("A:%+6.1f,%+6.1f,%+6.1f \r\n", fa[0], fa[1], fa[2]);
+    lcd1.printf("%d\r\n", count++);
+#endif
 }
 
-// Read angle and control an inertia rotor
-void rotor_control(void const *args){
-    // Input angle range
-    controller.setInputLimits(-90.0, 90.0);
-    // Output motor speed
-    controller.setOutputLimits(-50, 50);
-    // a bias.
-    controller.setBias(0.0);
-    controller.setMode(AUTO_MODE);
-    // Target
-    controller.setSetPoint(0.0);
-    while (true) {
-        osEvent evt = queue1.get();
-        // Update the process variable.
-        if ((kalAngleX < 0.8) && (kalAngleX > -0.8)){
-            angle = 0;
-        } else {
-            angle = kalAngleX;
-        }
-        controller.setProcessValue(angle);
-        // Set the new output.
-        stp = controller.compute() * 5;
-        rotor.move((int32_t)stp);
-    }
+void send_pc(void){
+    PRINTF("G:%+6.1f,%+6.1f,%+6.1f,  ", fg[0], fg[1], fg[2]);
+    PRINTF("A:%+6.1f,%+6.1f,%+6.1f \r\n", fa[0], fa[1], fa[2]);
 }
 
-// Update sensor data
-void display(void const *args){
+int main(void) {
     // SPI LCD
+#ifdef USE_SPI_LCD
     spi_lcd.frequency(100000);
     lcd1.cls();
     lcd1.set_contrast(0x2a);
@@ -220,68 +153,28 @@
     lcd1.circle(5,35,5,1);
     lcd1.fillcircle(60,55,5,1);
     lcd1.line(0,30,127,63,1);
+#endif
+    // I2C LCD
+#ifdef USE_I2C_LCD
+    lcd0.locate(0, 0);    // 1st line top
+    lcd0.printf("I2C test");
+    lcd0.locate(0, 1);    // 2nd line top
+    lcd0.puts(" JH1PJL ");
+    lcd0.setContrast(0x14);
+#endif
+    count = 0; 
     while (true) {
-        Thread::wait(500);
+        update_angle();
+        display();
+        send_pc();
+        //wait(0.2);
+        blink((void *)0);
+        //wait(0.2);
+        blink((void *)1);
+        //wait(0.2);
+        blink((void *)2);
+        //wait(0.2);
+        blink((void *)3);
+        //wait(0.2);
     }
 }
-
-// Thread definition
-osThreadDef(update_angle, osPriorityRealtime, 4096);
-osThreadDef(rotor_control, osPriorityAboveNormal, 4096);
-osThreadDef(monitor, osPriorityNormal, 4096);
-osThreadDef(display, osPriorityNormal, 4096);
-
-int main(void) {
-    PRINTF("step1\r\n");
-    
-    RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
-    RtosTimer led_2_timer(blink, osTimerPeriodic, (void *)1);
-    RtosTimer led_3_timer(blink, osTimerPeriodic, (void *)2);
-    RtosTimer led_4_timer(blink, osTimerPeriodic, (void *)3);
-
-    PRINTF("step2\r\n");    
-    led_1_timer.start(2000);
-    led_2_timer.start(1000);
-    led_3_timer.start(500);
-    led_4_timer.start(250);
-
-    PRINTF("step3\r\n");    
-    Thread thread(send_thread);
-
-    PRINTF("step4\r\n");
-    // Initialize data
-    stp = 0;
-    angle = 0.0;
-
-    // IRQ
-    Ticker ticker0;
-    Ticker ticker1;
-    ticker0.attach(queue_isr0, TIME_BASE_S);
-    ticker1.attach(queue_isr1, RATE);
-    rotor.set_max_speed(TIMEBASE);
-    
-    PRINTF("step5\r\n");
-    // Starts 1st thread
-    osThreadCreate(osThread(update_angle), NULL);
-    // Starts 2nd thread
-    osThreadCreate(osThread(rotor_control), NULL);
-    // Starts 3rd thread
-    osThreadCreate(osThread(monitor), NULL);
-    // Starts 4th thread
-    osThreadCreate(osThread(display), NULL);
-
-    PRINTF("step6\r\n");   
-    while (true) {
-        osEvent evt = mail_box.get();
-        if (evt.status == osEventMail) {
-            mail_t *mail = (mail_t*)evt.value.p;
-            if (show_flag){
-                PRINTF("This is dummy!, ");
-                PRINTF("Volt: %.2f V, "   , mail->voltage);
-                PRINTF("Current: %.2f A, "     , mail->current);
-                PRINTF("# of cycles: %u\r\n", mail->counter);
-            }
-            mail_box.free(mail);
-        }
-    }
-}