use mbed-src latest version and everything works well. RTC is also fine.
Dependencies: L3GD20 LIS3DH TextLCD mbed-rtos mbed
Use standard library mbed & mbed-rtos (GR-PEACH can run without mbed-src and special mbed-rtos).
Diff: main.cpp
- Revision:
- 5:dccdaaa1e57b
- Parent:
- 4:76b3113c79ff
- Child:
- 6:f14cce59e7fe
--- 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); + } +}