Diff: main.cpp

Revision:

4:76b3113c79ff

Parent:

3:989d13762f43

Child:

5:dccdaaa1e57b

--- a/main.cpp	Thu Dec 04 12:07:23 2014 +0000
+++ b/main.cpp	Sun Dec 14 09:17:01 2014 +0000
@@ -6,7 +6,7 @@
  *  http://www.page.sannet.ne.jp/kenjia/index.html
  *  http://mbed.org/users/kenjiArai/
  *      Created: November  29th, 2014
- *      Revised: November  29th, 2014
+ *      Revised: December  14th, 2014
  *
  * 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
@@ -15,148 +15,273 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-#include "mbed.h"
-
-//#define PIN_NUM
-#define LED_NAME
-//#define LED_COLOR
-
-#define DISP_COM
-
-// Com
-#ifdef DISP_COM
-#define BAUD(x)           pcm.baud(x)
-#define PRINTF(...)       pcm.printf(__VA_ARGS__)
-#else
-#define BAUD(x)           baud(x)
-#define PRINTF(...)       printf(__VA_ARGS__)
-#endif
+//  Include ---------------------------------------------------------------------------------------
+#include    "mbed.h"
+#include    "rtos.h"
+#include    "L3GD20.h"
+#include    "LIS3DH.h"
+#include    "ST7565_SPI_LCD.h"
+#include    "PID.h"
+#include    "stepper.h"
 
-#ifdef DISP_COM
-// com
-Serial pcm(USBTX, USBRX);
-#endif
-
-#if defined(PIN_NUM)
-DigitalOut myledR(P4_4);
-DigitalOut myledG(P3_2);
-DigitalOut myledB(P4_6);
-DigitalOut myledU(P4_7);
-#elif defined(LED_NAME)
-DigitalOut myledR(LED1);
-DigitalOut myledG(LED2);
-DigitalOut myledB(LED3);
-DigitalOut myledU(LED4);
-#elif defined(LED_COLOR)
-DigitalOut myledR(LED_RED);
-DigitalOut myledG(LED_GREEN);
-DigitalOut myledB(LED_BLUE);
-DigitalOut myledU(LED_USER);
+//  Definition ------------------------------------------------------------------------------------
+#define USE_COM         // use Communication with PC(UART)
+ 
+// Com
+#ifdef  USE_COM
+#define BAUD(x)             pcx.baud(x)
+#define GETC(x)             pcx.getc(x)
+#define PUTC(x)             pcx.putc(x)
+#define PRINTF(...)         pcx.printf(__VA_ARGS__)
+#define READABLE(x)         pcx.readable(x)
+#else
+#define BAUD(x)             {;}
+#define GETC(x)             {;}
+#define PUTC(x)             {;}
+#define PRINTF(...)         {;}
+#define READABLE(x)         {;}
 #endif
 
-#define ON  0
-#define OFF 1
+#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] = {
+    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
+};
+// Swiches
+DigitalIn   USER_SWITCH[2] = {
+    #if defined(TARGET_RZ_A1H)
+    DigitalIn(P6_0),  DigitalIn(P6_1)
+    #elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)\
+     || defined(TARGET_NUCLEO_L152RE)
+    DigitalIn(PC_13),  DigitalIn(A0)
+    #elif defined(TARGET_LPC1768)
+    DigitalIn(A0),  DigitalIn(A1)
+    #elif defined(TARGET_K64F)
+    DigitalIn(PTA4),  DigitalIn(PTC6)
+    #endif
+};
+// Rotor
+STEPPER     rotor(D5, D4, D3, D2);
+// com
+#ifdef USE_COM
+Serial      pcx(USBTX, USBRX);      // Communication with Host
+#endif
+I2C i2c(D14,D15);
+// Gyro
+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);
+// 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; 
 
-int main() {
-    uint32_t n = 0;
-    
-    PRINTF("Start mbed program\r\n");
-    myledR = OFF;
-    PRINTF("RF,");
-    myledG = OFF;
-    PRINTF("GF,");
-    myledB = OFF;
-    PRINTF("BF,");
-    myledU = ON;
-    PRINTF("UN");
-    while(1) {
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=ON ,G=OFF,B=OFF ");
-        myledR = ON;
-        PRINTF("RN,");
-        myledG = OFF;
-        PRINTF("GF,");
-        myledB = OFF;
-        PRINTF("BF,");
-        myledU = OFF;
-        PRINTF("UF");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=OFF,G=OFF,B=OFF ");
-        myledR = OFF;
-        PRINTF("RF,");
-        myledG = OFF;
-        PRINTF("GF,");
-        myledB = OFF;
-        PRINTF("BF,");
-        myledU = ON;
-        PRINTF("UN");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=OFF,G=ON ,B=OFF ");
-        myledR = OFF;
-        PRINTF("RF,");
-        myledG = ON;
-        PRINTF("GN,");
-        myledB = OFF;
-        PRINTF("BF,");
-        myledU = OFF;
-        PRINTF("UF");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=OFF,G=OFF,B=OFF ");
-        myledR = OFF;
-        PRINTF("RF,");
-        myledG = OFF;
-        PRINTF("GF,");
-        myledB = OFF;
-        PRINTF("BF,");
-        myledU = ON;
-        PRINTF("UN");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=OFF,G=OFF,B=ON  ");
-        myledR = OFF;
-        PRINTF("RF,");
-        myledG = OFF;
-        PRINTF("GF,");
-        myledB = ON;
-        PRINTF("BN,");
-        myledU = OFF;
-        PRINTF("UF");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=OFF,G=OFF,B=OFF ");
-        myledR = OFF;
-        PRINTF("RF,");
-        myledG = OFF;
-        PRINTF("GF,");
-        myledB = OFF;
-        PRINTF("BF,");
-        myledU = ON;
-        PRINTF("UN");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=ON ,G=ON ,B=ON  ");
-        myledR = ON;
-        PRINTF("RN,");
-        myledG = ON;
-        PRINTF("GN,");
-        myledB = ON;
-        PRINTF("BN,");
-        myledU = OFF;
-        PRINTF("UF");
-        wait(5.0);
-        PRINTF(" passed %4d Sec\r\n", n +=5);
-        PRINTF("R=OFF,G=OFF,B=OFF ");
-        myledR = OFF;
-        PRINTF("RF,");
-        myledG = OFF;
-        PRINTF("GF,");
-        myledB = OFF;
-        PRINTF("BF,");
-        myledU = ON;
-        PRINTF("UN");
+//  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;
+
+//  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];
+}
+
+// Read switch status
+int read_sw(uint8_t n){
+    if (USER_SWITCH[n] == 0){   return 1;
+    } 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 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;
+    }
+}
+
+// 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);
+    }
+}
+
+// Update sensor data
+void display(void const *args){
+    // 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);
+    while (true) {
+        Thread::wait(500);
+    }
+}
+
+// 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);
+        }
+    }
+}