Kenji Arai
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);
- }
- }
-}