Monitor program only for mbed GR-PEACH
main.cpp@4:76b3113c79ff, 2014-12-14 (annotated)
- Committer:
- kenjiArai
- Date:
- Sun Dec 14 09:17:01 2014 +0000
- Revision:
- 4:76b3113c79ff
- Parent:
- 3:989d13762f43
- Child:
- 5:1d465d550109
Check rtos functions on GR-PEACH. Does NOT work on GR-PEACH but does work on LPC1768, F401RE and L152RE.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenjiArai | 1:a399a743d109 | 1 | /* |
kenjiArai | 1:a399a743d109 | 2 | * mbed Application program for the mbed |
kenjiArai | 1:a399a743d109 | 3 | * Test program for GR-PEACH |
kenjiArai | 1:a399a743d109 | 4 | * |
kenjiArai | 1:a399a743d109 | 5 | * Copyright (c) 2014 Kenji Arai / JH1PJL |
kenjiArai | 1:a399a743d109 | 6 | * http://www.page.sannet.ne.jp/kenjia/index.html |
kenjiArai | 1:a399a743d109 | 7 | * http://mbed.org/users/kenjiArai/ |
kenjiArai | 1:a399a743d109 | 8 | * Created: November 29th, 2014 |
kenjiArai | 4:76b3113c79ff | 9 | * Revised: December 14th, 2014 |
kenjiArai | 1:a399a743d109 | 10 | * |
kenjiArai | 1:a399a743d109 | 11 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
kenjiArai | 1:a399a743d109 | 12 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE |
kenjiArai | 1:a399a743d109 | 13 | * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
kenjiArai | 1:a399a743d109 | 14 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
kenjiArai | 1:a399a743d109 | 15 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
kenjiArai | 1:a399a743d109 | 16 | */ |
kenjiArai | 1:a399a743d109 | 17 | |
kenjiArai | 4:76b3113c79ff | 18 | // Include --------------------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 19 | #include "mbed.h" |
kenjiArai | 4:76b3113c79ff | 20 | #include "rtos.h" |
kenjiArai | 4:76b3113c79ff | 21 | #include "L3GD20.h" |
kenjiArai | 4:76b3113c79ff | 22 | #include "LIS3DH.h" |
kenjiArai | 4:76b3113c79ff | 23 | #include "ST7565_SPI_LCD.h" |
kenjiArai | 4:76b3113c79ff | 24 | #include "PID.h" |
kenjiArai | 4:76b3113c79ff | 25 | #include "stepper.h" |
kenjiArai | 2:3747397a1618 | 26 | |
kenjiArai | 4:76b3113c79ff | 27 | // Definition ------------------------------------------------------------------------------------ |
kenjiArai | 4:76b3113c79ff | 28 | #define USE_COM // use Communication with PC(UART) |
kenjiArai | 4:76b3113c79ff | 29 | |
kenjiArai | 4:76b3113c79ff | 30 | // Com |
kenjiArai | 4:76b3113c79ff | 31 | #ifdef USE_COM |
kenjiArai | 4:76b3113c79ff | 32 | #define BAUD(x) pcx.baud(x) |
kenjiArai | 4:76b3113c79ff | 33 | #define GETC(x) pcx.getc(x) |
kenjiArai | 4:76b3113c79ff | 34 | #define PUTC(x) pcx.putc(x) |
kenjiArai | 4:76b3113c79ff | 35 | #define PRINTF(...) pcx.printf(__VA_ARGS__) |
kenjiArai | 4:76b3113c79ff | 36 | #define READABLE(x) pcx.readable(x) |
kenjiArai | 4:76b3113c79ff | 37 | #else |
kenjiArai | 4:76b3113c79ff | 38 | #define BAUD(x) {;} |
kenjiArai | 4:76b3113c79ff | 39 | #define GETC(x) {;} |
kenjiArai | 4:76b3113c79ff | 40 | #define PUTC(x) {;} |
kenjiArai | 4:76b3113c79ff | 41 | #define PRINTF(...) {;} |
kenjiArai | 4:76b3113c79ff | 42 | #define READABLE(x) {;} |
kenjiArai | 0:a17b3cf2f466 | 43 | #endif |
kenjiArai | 0:a17b3cf2f466 | 44 | |
kenjiArai | 4:76b3113c79ff | 45 | #define TIMEBASE 12000 |
kenjiArai | 4:76b3113c79ff | 46 | #define FIXED_STEPS 100 |
kenjiArai | 4:76b3113c79ff | 47 | |
kenjiArai | 4:76b3113c79ff | 48 | #define PI 3.1415926536 |
kenjiArai | 4:76b3113c79ff | 49 | #define RAD_TO_DEG 57.29578 |
kenjiArai | 4:76b3113c79ff | 50 | #define TIME_BASE_S 0.01 |
kenjiArai | 4:76b3113c79ff | 51 | #define TIME_BASE_MS ( TIME_BASE_S * 1000) |
kenjiArai | 4:76b3113c79ff | 52 | #define RATE 0.1 |
kenjiArai | 4:76b3113c79ff | 53 | |
kenjiArai | 4:76b3113c79ff | 54 | // Object ---------------------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 55 | // LED's |
kenjiArai | 4:76b3113c79ff | 56 | DigitalOut LEDs[4] = { |
kenjiArai | 4:76b3113c79ff | 57 | DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4) |
kenjiArai | 4:76b3113c79ff | 58 | }; |
kenjiArai | 4:76b3113c79ff | 59 | // Swiches |
kenjiArai | 4:76b3113c79ff | 60 | DigitalIn USER_SWITCH[2] = { |
kenjiArai | 4:76b3113c79ff | 61 | #if defined(TARGET_RZ_A1H) |
kenjiArai | 4:76b3113c79ff | 62 | DigitalIn(P6_0), DigitalIn(P6_1) |
kenjiArai | 4:76b3113c79ff | 63 | #elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)\ |
kenjiArai | 4:76b3113c79ff | 64 | || defined(TARGET_NUCLEO_L152RE) |
kenjiArai | 4:76b3113c79ff | 65 | DigitalIn(PC_13), DigitalIn(A0) |
kenjiArai | 4:76b3113c79ff | 66 | #elif defined(TARGET_LPC1768) |
kenjiArai | 4:76b3113c79ff | 67 | DigitalIn(A0), DigitalIn(A1) |
kenjiArai | 4:76b3113c79ff | 68 | #elif defined(TARGET_K64F) |
kenjiArai | 4:76b3113c79ff | 69 | DigitalIn(PTA4), DigitalIn(PTC6) |
kenjiArai | 4:76b3113c79ff | 70 | #endif |
kenjiArai | 4:76b3113c79ff | 71 | }; |
kenjiArai | 4:76b3113c79ff | 72 | // Rotor |
kenjiArai | 4:76b3113c79ff | 73 | STEPPER rotor(D5, D4, D3, D2); |
kenjiArai | 4:76b3113c79ff | 74 | // com |
kenjiArai | 4:76b3113c79ff | 75 | #ifdef USE_COM |
kenjiArai | 4:76b3113c79ff | 76 | Serial pcx(USBTX, USBRX); // Communication with Host |
kenjiArai | 4:76b3113c79ff | 77 | #endif |
kenjiArai | 4:76b3113c79ff | 78 | I2C i2c(D14,D15); |
kenjiArai | 4:76b3113c79ff | 79 | // Gyro |
kenjiArai | 4:76b3113c79ff | 80 | L3GX_GYRO gyro(i2c, L3GD20_V_CHIP_ADDR, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS); |
kenjiArai | 4:76b3113c79ff | 81 | // Acc |
kenjiArai | 4:76b3113c79ff | 82 | LIS3DH acc(i2c, LIS3DH_G_CHIP_ADDR, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G); |
kenjiArai | 4:76b3113c79ff | 83 | // SPI LCD |
kenjiArai | 4:76b3113c79ff | 84 | SPI spi_lcd(D11, D12, D13); // mosi, miso, sck |
kenjiArai | 4:76b3113c79ff | 85 | ST7565 lcd1(spi_lcd, D8, D9, D7, ST7565::AQM1248A); // spi,reset,a0,ncs, LCD(Akizuki AQM1248A) |
kenjiArai | 4:76b3113c79ff | 86 | // Kc, Ti, Td, interval |
kenjiArai | 4:76b3113c79ff | 87 | PID controller(1.0, 0.0, 0.0, RATE); |
kenjiArai | 4:76b3113c79ff | 88 | // Mutex |
kenjiArai | 4:76b3113c79ff | 89 | Mutex i2c_mutex; |
kenjiArai | 0:a17b3cf2f466 | 90 | |
kenjiArai | 4:76b3113c79ff | 91 | // RAM ------------------------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 92 | Queue<uint32_t, 2> queue0; |
kenjiArai | 4:76b3113c79ff | 93 | Queue<uint32_t, 2> queue1; |
kenjiArai | 4:76b3113c79ff | 94 | float fa[3]; // Acc 0:X, 1:Y, 2:Z |
kenjiArai | 4:76b3113c79ff | 95 | float fg[3]; // Gyro 0:X, 1:Y, 2:Z |
kenjiArai | 4:76b3113c79ff | 96 | float accXangle; // Angle calculate using the accelerometer |
kenjiArai | 4:76b3113c79ff | 97 | float gyroXangle; // Angle calculate using the gyro |
kenjiArai | 4:76b3113c79ff | 98 | float kalAngleX; // Calculate the angle using a Kalman filter |
kenjiArai | 4:76b3113c79ff | 99 | float stp; |
kenjiArai | 4:76b3113c79ff | 100 | float angle; |
kenjiArai | 4:76b3113c79ff | 101 | |
kenjiArai | 4:76b3113c79ff | 102 | uint8_t pls_width[MT_SLOP_STEP] = {5, 4, 3, 2, 1, 1, 1, 1, 1, 1 }; |
kenjiArai | 4:76b3113c79ff | 103 | |
kenjiArai | 4:76b3113c79ff | 104 | /* Mail */ |
kenjiArai | 4:76b3113c79ff | 105 | typedef struct { |
kenjiArai | 4:76b3113c79ff | 106 | float voltage; /* AD result of measured voltage */ |
kenjiArai | 4:76b3113c79ff | 107 | float current; /* AD result of measured current */ |
kenjiArai | 4:76b3113c79ff | 108 | uint32_t counter; /* A counter value */ |
kenjiArai | 4:76b3113c79ff | 109 | } mail_t; |
kenjiArai | 4:76b3113c79ff | 110 | |
kenjiArai | 4:76b3113c79ff | 111 | Mail<mail_t, 16> mail_box; |
kenjiArai | 4:76b3113c79ff | 112 | |
kenjiArai | 4:76b3113c79ff | 113 | uint8_t show_flag; |
kenjiArai | 4:76b3113c79ff | 114 | |
kenjiArai | 4:76b3113c79ff | 115 | // ROM / Constant data --------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 116 | |
kenjiArai | 4:76b3113c79ff | 117 | // Function prototypes --------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 118 | |
kenjiArai | 4:76b3113c79ff | 119 | // Function prototypes --------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 120 | extern int mon( void); |
kenjiArai | 4:76b3113c79ff | 121 | extern float kalmanCalculate(float newAngle, float newRate, int looptime); |
kenjiArai | 4:76b3113c79ff | 122 | |
kenjiArai | 4:76b3113c79ff | 123 | //------------------------------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 124 | // Control Program |
kenjiArai | 4:76b3113c79ff | 125 | //------------------------------------------------------------------------------------------------- |
kenjiArai | 4:76b3113c79ff | 126 | void send_thread (void const *args) { |
kenjiArai | 4:76b3113c79ff | 127 | uint32_t i = 0; |
kenjiArai | 4:76b3113c79ff | 128 | while (true) { |
kenjiArai | 4:76b3113c79ff | 129 | i++; // fake data update |
kenjiArai | 4:76b3113c79ff | 130 | mail_t *mail = mail_box.alloc(); |
kenjiArai | 4:76b3113c79ff | 131 | mail->voltage = (i * 0.1) * 33; |
kenjiArai | 4:76b3113c79ff | 132 | mail->current = (i * 0.1) * 11; |
kenjiArai | 4:76b3113c79ff | 133 | mail->counter = i; |
kenjiArai | 4:76b3113c79ff | 134 | mail_box.put(mail); |
kenjiArai | 4:76b3113c79ff | 135 | Thread::wait(1000); |
kenjiArai | 4:76b3113c79ff | 136 | } |
kenjiArai | 4:76b3113c79ff | 137 | } |
kenjiArai | 4:76b3113c79ff | 138 | |
kenjiArai | 4:76b3113c79ff | 139 | void blink(void const *n) { |
kenjiArai | 4:76b3113c79ff | 140 | LEDs[(int)n] = !LEDs[(int)n]; |
kenjiArai | 4:76b3113c79ff | 141 | } |
kenjiArai | 4:76b3113c79ff | 142 | |
kenjiArai | 4:76b3113c79ff | 143 | // Read switch status |
kenjiArai | 4:76b3113c79ff | 144 | int read_sw(uint8_t n){ |
kenjiArai | 4:76b3113c79ff | 145 | if (USER_SWITCH[n] == 0){ return 1; |
kenjiArai | 4:76b3113c79ff | 146 | } else { return 0;} |
kenjiArai | 4:76b3113c79ff | 147 | } |
kenjiArai | 4:76b3113c79ff | 148 | |
kenjiArai | 4:76b3113c79ff | 149 | // Monitor program |
kenjiArai | 4:76b3113c79ff | 150 | void monitor(void const *args){ |
kenjiArai | 4:76b3113c79ff | 151 | while (true){ |
kenjiArai | 4:76b3113c79ff | 152 | mon(); |
kenjiArai | 0:a17b3cf2f466 | 153 | } |
kenjiArai | 0:a17b3cf2f466 | 154 | } |
kenjiArai | 4:76b3113c79ff | 155 | |
kenjiArai | 4:76b3113c79ff | 156 | // Interrupt routine |
kenjiArai | 4:76b3113c79ff | 157 | void queue_isr0() { |
kenjiArai | 4:76b3113c79ff | 158 | queue0.put((uint32_t*)1); |
kenjiArai | 4:76b3113c79ff | 159 | } |
kenjiArai | 4:76b3113c79ff | 160 | |
kenjiArai | 4:76b3113c79ff | 161 | void queue_isr1() { |
kenjiArai | 4:76b3113c79ff | 162 | queue1.put((uint32_t*)1); |
kenjiArai | 4:76b3113c79ff | 163 | } |
kenjiArai | 4:76b3113c79ff | 164 | |
kenjiArai | 4:76b3113c79ff | 165 | // Update sensor data |
kenjiArai | 4:76b3113c79ff | 166 | void update_angle(void const *args){ |
kenjiArai | 4:76b3113c79ff | 167 | while (true) { |
kenjiArai | 4:76b3113c79ff | 168 | osEvent evt = queue0.get(); |
kenjiArai | 4:76b3113c79ff | 169 | // ---->lock |
kenjiArai | 4:76b3113c79ff | 170 | i2c_mutex.lock(); |
kenjiArai | 4:76b3113c79ff | 171 | // read acceleration data from sensor |
kenjiArai | 4:76b3113c79ff | 172 | acc.read_data(fa); |
kenjiArai | 4:76b3113c79ff | 173 | // read gyroscope data from sensor |
kenjiArai | 4:76b3113c79ff | 174 | gyro.read_data(fg); |
kenjiArai | 4:76b3113c79ff | 175 | // <----unlock |
kenjiArai | 4:76b3113c79ff | 176 | i2c_mutex.unlock(); |
kenjiArai | 4:76b3113c79ff | 177 | // Calculate angle (degree) |
kenjiArai | 4:76b3113c79ff | 178 | accXangle = (atan2(-fa[1],fa[2])+PI)*RAD_TO_DEG; |
kenjiArai | 4:76b3113c79ff | 179 | // calculate filtered Angle |
kenjiArai | 4:76b3113c79ff | 180 | kalAngleX = kalmanCalculate(accXangle, fg[0], TIME_BASE_MS) - 180; |
kenjiArai | 4:76b3113c79ff | 181 | } |
kenjiArai | 4:76b3113c79ff | 182 | } |
kenjiArai | 4:76b3113c79ff | 183 | |
kenjiArai | 4:76b3113c79ff | 184 | // Read angle and control an inertia rotor |
kenjiArai | 4:76b3113c79ff | 185 | void rotor_control(void const *args){ |
kenjiArai | 4:76b3113c79ff | 186 | // Input angle range |
kenjiArai | 4:76b3113c79ff | 187 | controller.setInputLimits(-90.0, 90.0); |
kenjiArai | 4:76b3113c79ff | 188 | // Output motor speed |
kenjiArai | 4:76b3113c79ff | 189 | controller.setOutputLimits(-50, 50); |
kenjiArai | 4:76b3113c79ff | 190 | // a bias. |
kenjiArai | 4:76b3113c79ff | 191 | controller.setBias(0.0); |
kenjiArai | 4:76b3113c79ff | 192 | controller.setMode(AUTO_MODE); |
kenjiArai | 4:76b3113c79ff | 193 | // Target |
kenjiArai | 4:76b3113c79ff | 194 | controller.setSetPoint(0.0); |
kenjiArai | 4:76b3113c79ff | 195 | while (true) { |
kenjiArai | 4:76b3113c79ff | 196 | osEvent evt = queue1.get(); |
kenjiArai | 4:76b3113c79ff | 197 | // Update the process variable. |
kenjiArai | 4:76b3113c79ff | 198 | if ((kalAngleX < 0.8) && (kalAngleX > -0.8)){ |
kenjiArai | 4:76b3113c79ff | 199 | angle = 0; |
kenjiArai | 4:76b3113c79ff | 200 | } else { |
kenjiArai | 4:76b3113c79ff | 201 | angle = kalAngleX; |
kenjiArai | 4:76b3113c79ff | 202 | } |
kenjiArai | 4:76b3113c79ff | 203 | controller.setProcessValue(angle); |
kenjiArai | 4:76b3113c79ff | 204 | // Set the new output. |
kenjiArai | 4:76b3113c79ff | 205 | stp = controller.compute() * 5; |
kenjiArai | 4:76b3113c79ff | 206 | rotor.move((int32_t)stp); |
kenjiArai | 4:76b3113c79ff | 207 | } |
kenjiArai | 4:76b3113c79ff | 208 | } |
kenjiArai | 4:76b3113c79ff | 209 | |
kenjiArai | 4:76b3113c79ff | 210 | // Update sensor data |
kenjiArai | 4:76b3113c79ff | 211 | void display(void const *args){ |
kenjiArai | 4:76b3113c79ff | 212 | // SPI LCD |
kenjiArai | 4:76b3113c79ff | 213 | spi_lcd.frequency(100000); |
kenjiArai | 4:76b3113c79ff | 214 | lcd1.cls(); |
kenjiArai | 4:76b3113c79ff | 215 | lcd1.set_contrast(0x2a); |
kenjiArai | 4:76b3113c79ff | 216 | lcd1.printf("test\r\n" ); |
kenjiArai | 4:76b3113c79ff | 217 | lcd1.printf("Kenji Arai / JH1PJL\r\n" ); |
kenjiArai | 4:76b3113c79ff | 218 | lcd1.printf("ABCDEFG 1234567890\r\n" ); |
kenjiArai | 4:76b3113c79ff | 219 | lcd1.rect(5,30,120,62,1); |
kenjiArai | 4:76b3113c79ff | 220 | lcd1.circle(5,35,5,1); |
kenjiArai | 4:76b3113c79ff | 221 | lcd1.fillcircle(60,55,5,1); |
kenjiArai | 4:76b3113c79ff | 222 | lcd1.line(0,30,127,63,1); |
kenjiArai | 4:76b3113c79ff | 223 | while (true) { |
kenjiArai | 4:76b3113c79ff | 224 | Thread::wait(500); |
kenjiArai | 4:76b3113c79ff | 225 | } |
kenjiArai | 4:76b3113c79ff | 226 | } |
kenjiArai | 4:76b3113c79ff | 227 | |
kenjiArai | 4:76b3113c79ff | 228 | // Thread definition |
kenjiArai | 4:76b3113c79ff | 229 | osThreadDef(update_angle, osPriorityRealtime, 4096); |
kenjiArai | 4:76b3113c79ff | 230 | osThreadDef(rotor_control, osPriorityAboveNormal, 4096); |
kenjiArai | 4:76b3113c79ff | 231 | osThreadDef(monitor, osPriorityNormal, 4096); |
kenjiArai | 4:76b3113c79ff | 232 | osThreadDef(display, osPriorityNormal, 4096); |
kenjiArai | 4:76b3113c79ff | 233 | |
kenjiArai | 4:76b3113c79ff | 234 | int main(void) { |
kenjiArai | 4:76b3113c79ff | 235 | PRINTF("step1\r\n"); |
kenjiArai | 4:76b3113c79ff | 236 | |
kenjiArai | 4:76b3113c79ff | 237 | RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0); |
kenjiArai | 4:76b3113c79ff | 238 | RtosTimer led_2_timer(blink, osTimerPeriodic, (void *)1); |
kenjiArai | 4:76b3113c79ff | 239 | RtosTimer led_3_timer(blink, osTimerPeriodic, (void *)2); |
kenjiArai | 4:76b3113c79ff | 240 | RtosTimer led_4_timer(blink, osTimerPeriodic, (void *)3); |
kenjiArai | 4:76b3113c79ff | 241 | |
kenjiArai | 4:76b3113c79ff | 242 | PRINTF("step2\r\n"); |
kenjiArai | 4:76b3113c79ff | 243 | led_1_timer.start(2000); |
kenjiArai | 4:76b3113c79ff | 244 | led_2_timer.start(1000); |
kenjiArai | 4:76b3113c79ff | 245 | led_3_timer.start(500); |
kenjiArai | 4:76b3113c79ff | 246 | led_4_timer.start(250); |
kenjiArai | 4:76b3113c79ff | 247 | |
kenjiArai | 4:76b3113c79ff | 248 | PRINTF("step3\r\n"); |
kenjiArai | 4:76b3113c79ff | 249 | Thread thread(send_thread); |
kenjiArai | 4:76b3113c79ff | 250 | |
kenjiArai | 4:76b3113c79ff | 251 | PRINTF("step4\r\n"); |
kenjiArai | 4:76b3113c79ff | 252 | // Initialize data |
kenjiArai | 4:76b3113c79ff | 253 | stp = 0; |
kenjiArai | 4:76b3113c79ff | 254 | angle = 0.0; |
kenjiArai | 4:76b3113c79ff | 255 | |
kenjiArai | 4:76b3113c79ff | 256 | // IRQ |
kenjiArai | 4:76b3113c79ff | 257 | Ticker ticker0; |
kenjiArai | 4:76b3113c79ff | 258 | Ticker ticker1; |
kenjiArai | 4:76b3113c79ff | 259 | ticker0.attach(queue_isr0, TIME_BASE_S); |
kenjiArai | 4:76b3113c79ff | 260 | ticker1.attach(queue_isr1, RATE); |
kenjiArai | 4:76b3113c79ff | 261 | rotor.set_max_speed(TIMEBASE); |
kenjiArai | 4:76b3113c79ff | 262 | |
kenjiArai | 4:76b3113c79ff | 263 | PRINTF("step5\r\n"); |
kenjiArai | 4:76b3113c79ff | 264 | // Starts 1st thread |
kenjiArai | 4:76b3113c79ff | 265 | osThreadCreate(osThread(update_angle), NULL); |
kenjiArai | 4:76b3113c79ff | 266 | // Starts 2nd thread |
kenjiArai | 4:76b3113c79ff | 267 | osThreadCreate(osThread(rotor_control), NULL); |
kenjiArai | 4:76b3113c79ff | 268 | // Starts 3rd thread |
kenjiArai | 4:76b3113c79ff | 269 | osThreadCreate(osThread(monitor), NULL); |
kenjiArai | 4:76b3113c79ff | 270 | // Starts 4th thread |
kenjiArai | 4:76b3113c79ff | 271 | osThreadCreate(osThread(display), NULL); |
kenjiArai | 4:76b3113c79ff | 272 | |
kenjiArai | 4:76b3113c79ff | 273 | PRINTF("step6\r\n"); |
kenjiArai | 4:76b3113c79ff | 274 | while (true) { |
kenjiArai | 4:76b3113c79ff | 275 | osEvent evt = mail_box.get(); |
kenjiArai | 4:76b3113c79ff | 276 | if (evt.status == osEventMail) { |
kenjiArai | 4:76b3113c79ff | 277 | mail_t *mail = (mail_t*)evt.value.p; |
kenjiArai | 4:76b3113c79ff | 278 | if (show_flag){ |
kenjiArai | 4:76b3113c79ff | 279 | PRINTF("This is dummy!, "); |
kenjiArai | 4:76b3113c79ff | 280 | PRINTF("Volt: %.2f V, " , mail->voltage); |
kenjiArai | 4:76b3113c79ff | 281 | PRINTF("Current: %.2f A, " , mail->current); |
kenjiArai | 4:76b3113c79ff | 282 | PRINTF("# of cycles: %u\r\n", mail->counter); |
kenjiArai | 4:76b3113c79ff | 283 | } |
kenjiArai | 4:76b3113c79ff | 284 | mail_box.free(mail); |
kenjiArai | 4:76b3113c79ff | 285 | } |
kenjiArai | 4:76b3113c79ff | 286 | } |
kenjiArai | 4:76b3113c79ff | 287 | } |