Diff: main.cpp

Revision:

5:dccdaaa1e57b

Parent:

4:76b3113c79ff

Child:

6:f14cce59e7fe

diff -r 76b3113c79ff -r dccdaaa1e57b main.cpp
--- a/main.cpp	Sun Dec 14 09:17:01 2014 +0000
+++ b/main.cpp	Thu Jan 08 13:03:16 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    7th, 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
@@ -20,12 +20,13 @@
 #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_I2C_SENSOR
  
 // Com
 #ifdef  USE_COM
@@ -47,7 +48,7 @@
 
 #define PI              3.1415926536
 #define RAD_TO_DEG      57.29578
-#define TIME_BASE_S     0.01
+#define TIME_BASE_S     0.02
 #define TIME_BASE_MS    ( TIME_BASE_S * 1000)
 #define RATE            0.1
 
@@ -58,19 +59,22 @@
 };
 // Swiches
 DigitalIn   USER_SWITCH[2] = {
-    #if defined(TARGET_RZ_A1H)
+#if defined(TARGET_RZ_A1H)
     DigitalIn(P6_0),  DigitalIn(P6_1)
-    #elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)\
-     || defined(TARGET_NUCLEO_L152RE)
+#elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)
     DigitalIn(PC_13),  DigitalIn(A0)
-    #elif defined(TARGET_LPC1768)
-    DigitalIn(A0),  DigitalIn(A1)
-    #elif defined(TARGET_K64F)
-    DigitalIn(PTA4),  DigitalIn(PTC6)
-    #endif
+#endif
 };
-// Rotor
-STEPPER     rotor(D5, D4, D3, D2);
+
+// OS check
+#if defined(TARGET_RZ_A1H)
+DigitalOut task0(P1_8);
+DigitalOut task1(P1_9);
+#elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)
+DigitalOut task0(A0);
+DigitalOut task1(A1);
+#endif
+
 // com
 #ifdef USE_COM
 Serial      pcx(USBTX, USBRX);      // Communication with Host
@@ -80,26 +84,20 @@
 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
 // 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; 
 
 //  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 {
@@ -111,6 +109,12 @@
 Mail<mail_t, 16> mail_box;
 
 uint8_t show_flag;
+uint32_t count;
+
+#if defined(TARGET_RZ_A1H)
+uint8_t big_data[4096*1024];
+uint32_t big_data_index = 0;
+#endif
 
 //  ROM / Constant data ---------------------------------------------------------------------------
 
@@ -118,7 +122,6 @@
 
 //  Function prototypes ---------------------------------------------------------------------------
 extern int mon( void);
-extern float kalmanCalculate(float newAngle, float newRate, int looptime);
 
 //-------------------------------------------------------------------------------------------------
 //  Control Program
@@ -148,6 +151,7 @@
 
 // Monitor program
 void monitor(void const *args){
+    Thread::wait(1000);
     while (true){
         mon();
     }
@@ -158,81 +162,65 @@
     queue0.put((uint32_t*)1);
 }
 
-void queue_isr1() {
-    queue1.put((uint32_t*)1);
-}
-
 // Update sensor data
 void update_angle(void const *args){
     while (true) {
         osEvent evt = queue0.get();
         // ---->lock
         i2c_mutex.lock();
+#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
         // <----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;
-    }
-}
-
-// 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);
+        // debug
+        task0 = !task0;
     }
 }
 
 // Update sensor data
 void display(void const *args){
-    // SPI LCD
-    spi_lcd.frequency(100000);
+    uint32_t n = 0;
+
     lcd1.cls();
-    lcd1.set_contrast(0x2a);
-    lcd1.printf("test\r\n" );
-    lcd1.printf("Kenji Arai / JH1PJL\r\n" );
-    lcd1.printf("ABCDEFG 1234567890\r\n" );
-    lcd1.rect(5,30,120,62,1);
-    lcd1.circle(5,35,5,1);
-    lcd1.fillcircle(60,55,5,1);
-    lcd1.line(0,30,127,63,1);
     while (true) {
+#ifdef USE_I2C_LCD
+        i2c_mutex.lock();
+        lcd0.locate(0, 0);    // 1st line top
+        lcd0.printf("G=%7.1f ", sqrt(fa[0]*fa[0] + fa[1]*fa[1] + fa[2]*fa[2]));
+        lcd0.locate(0, 1);    // 2nd line top
+        lcd0.printf("%8d",n);
+        i2c_mutex.unlock();
+#endif
+        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", n++);
         Thread::wait(500);
+        // debug
+        task1 = !task1;
     }
 }
 
 // Thread definition
-osThreadDef(update_angle, osPriorityRealtime, 4096);
-osThreadDef(rotor_control, osPriorityAboveNormal, 4096);
-osThreadDef(monitor, osPriorityNormal, 4096);
-osThreadDef(display, osPriorityNormal, 4096);
+#if defined(TARGET_RZ_A1H)
+osThreadDef(update_angle, osPriorityNormal, 2048);
+osThreadDef(monitor, osPriorityNormal, 2048);
+osThreadDef(display, osPriorityAboveNormal, 2048);
+#elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)
+osThreadDef(update_angle, osPriorityNormal, 1024);
+osThreadDef(monitor, osPriorityNormal, 1024);
+osThreadDef(display, osPriorityAboveNormal, 1024);
+#endif
 
 int main(void) {
-    PRINTF("step1\r\n");
+    PRINTF("\r\nstep1\r\n");
     
     RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
     RtosTimer led_2_timer(blink, osTimerPeriodic, (void *)1);
@@ -249,27 +237,48 @@
     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);
+    // Starts threads
+    if (osThreadCreate(osThread(display), NULL) == NULL){
+        PRINTF("ERROR4\r\n");
+    }
+    if (osThreadCreate(osThread(monitor), NULL) == NULL){
+        PRINTF("ERROR3\r\n");
+    }
+    if (osThreadCreate(osThread(update_angle), NULL) == NULL){
+        PRINTF("ERROR1\r\n");
+    }
 
+    // SPI LCD
+    spi_lcd.frequency(100000);
+    lcd1.cls();
+    lcd1.set_contrast(0x2a);
+    lcd1.printf("test\r\n" );
+    lcd1.printf("Kenji Arai / JH1PJL\r\n" );
+    lcd1.printf("ABCDEFG 1234567890\r\n" );
+    lcd1.rect(5,30,120,62,1);
+    lcd1.circle(5,35,5,1);
+    lcd1.fillcircle(60,55,5,1);
+    lcd1.line(0,30,127,63,1);
+    // I2C LCD
+#ifdef USE_I2C_LCD
+    // ---->lock
+    i2c_mutex.lock();
+    lcd0.locate(0, 0);    // 1st line top
+    lcd0.printf("I2C test");
+    lcd0.locate(0, 1);    // 2nd line top
+    lcd0.puts(" JH1PJL ");
+    lcd0.setContrast(0x14);
+    // <----unlock
+    i2c_mutex.unlock();
+#endif
+    count = 0;
     PRINTF("step6\r\n");   
     while (true) {
         osEvent evt = mail_box.get();
@@ -283,5 +292,34 @@
             }
             mail_box.free(mail);
         }
+#if defined(TARGET_RZ_A1H)
+        for (uint32_t i = 0; i < 100; i++){
+            big_data[big_data_index] = ++big_data_index;
+            if (big_data_index == (4096 * 1024)){
+                big_data_index = 0;
+            }
+        }
+#endif
     }
 }
+
+void mbed_die(void) {
+    PRINTF("Error, came from os_error()!\r\n");
+    gpio_t led_1; gpio_init_out(&led_1, LED1);
+    gpio_t led_2; gpio_init_out(&led_2, LED2);
+    gpio_t led_3; gpio_init_out(&led_3, LED3);
+    gpio_t led_4; gpio_init_out(&led_4, LED4);
+
+    while (1) {
+        gpio_write(&led_1, 1);
+        gpio_write(&led_2, 0);
+        gpio_write(&led_3, 0);
+        gpio_write(&led_4, 1);
+        wait_ms(100);
+        gpio_write(&led_1, 0);
+        gpio_write(&led_2, 1);
+        gpio_write(&led_3, 1);
+        gpio_write(&led_4, 0);
+        wait_ms(100);
+    }
+}