Kenji Arai / GR-PEACH_test_wo_rtos

This is test purpose program only for GR-PEACH. This program only run one hour 11 minutes!

Dependencies:   L3GD20 LIS3DH TextLCD

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:e8d4095d9c19
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?

UserRevisionLine numberNew 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 }