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).
main.cpp
- Committer:
- kenjiArai
- Date:
- 2014-12-14
- Revision:
- 4:76b3113c79ff
- Parent:
- 3:989d13762f43
- Child:
- 5:dccdaaa1e57b
File content as of revision 4:76b3113c79ff:
/* * mbed Application program for the mbed * Test program for GR-PEACH * * Copyright (c) 2014 Kenji Arai / JH1PJL * http://www.page.sannet.ne.jp/kenjia/index.html * http://mbed.org/users/kenjiArai/ * Created: 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 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ // Include --------------------------------------------------------------------------------------- #include "mbed.h" #include "rtos.h" #include "L3GD20.h" #include "LIS3DH.h" #include "ST7565_SPI_LCD.h" #include "PID.h" #include "stepper.h" // 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 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; // 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); } } }