Monitor program only for mbed GR-PEACH
Diff: main.cpp
- Revision:
- 5:1d465d550109
- Parent:
- 4:76b3113c79ff
--- a/main.cpp Sun Dec 14 09:17:01 2014 +0000 +++ b/main.cpp Sun Dec 28 11:51:59 2014 +0000 @@ -5,8 +5,8 @@ * 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 + * Created: December 27th, 2014 + * Revised: December 28th, 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 @@ -17,271 +17,70 @@ // Include --------------------------------------------------------------------------------------- #include "mbed.h" -#include "rtos.h" -#include "L3GD20.h" -#include "LIS3DH.h" -#include "ST7565_SPI_LCD.h" -#include "PID.h" -#include "stepper.h" +// Object ---------------------------------------------------------------------------------------- +extern Serial pc(USBTX, USBRX); // 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 BAUD_RATE 9600 -#define TIMEBASE 12000 -#define FIXED_STEPS 100 +#define BAUD(x) pc.baud(x) +#define GETC(x) pc.getc(x) +#define PUTC(x) pc.putc(x) +#define PRINTF(...) pc.printf(__VA_ARGS__) +#define READABLE(x) pc.readable(x) -#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 +#define BUF_SIZE (128 * 1024) // 128KB +#define BUF_SIZE_SMALL (40 * 1024) // 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; +uint8_t data_buf0[BUF_SIZE]; +uint8_t data_buf1[BUF_SIZE]; +uint8_t data_buf2[BUF_SIZE]; +uint8_t data_buf3[BUF_SIZE_SMALL]; +//uint8_t data_buf2[BUF_SIZE] __attribute__ ((RW_DATA)); // ROM / Constant data --------------------------------------------------------------------------- // Function prototypes --------------------------------------------------------------------------- // Function prototypes --------------------------------------------------------------------------- -extern int mon( void); -extern float kalmanCalculate(float newAngle, float newRate, int looptime); +extern int mon_mem(void); //------------------------------------------------------------------------------------------------- // 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; - } +void print_data_address(void) +{ + PRINTF("data_buf0 addr top: 0x%8x\r\n", &data_buf0); + PRINTF("data_buf1 addr top: 0x%8x\r\n", &data_buf1); + PRINTF("data_buf2 addr top: 0x%8x\r\n", &data_buf2); + PRINTF("data_buf3 addr top: 0x%8x\r\n", &data_buf3); } -// 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); +int main() +{ + uint32_t n; + + PRINTF("\r\nSet data into data_buf0[]\r\n"); + for (n = 0; n < BUF_SIZE; n++) { + data_buf0[n] = n & 0xff; + } + PRINTF("Set data into data_buf1[]\r\n"); + for (n = 0; n < BUF_SIZE; n++) { + data_buf1[BUF_SIZE - n] = n & 0xff; + } + PRINTF("Set data into data_buf2[]\r\n"); + for (n = 0; n < BUF_SIZE; n++) { + data_buf2[n] = n & 0xff; + } + PRINTF("Set data into data_buf3[]\r\n"); + for (n = 0; n < BUF_SIZE_SMALL; n++) { + data_buf3[n] = n & 0xff; + } + print_data_address(); 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); + mon_mem(); } } - -// 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); - } - } -}