Eduardo Munoz Gutierrez / Mbed OS mbed-os-micromouse

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Motor

main.cpp@4:d8dc8544fe81, 2019-03-27 (annotated)

Committer:
edmugu
Date:
Wed Mar 27 04:20:42 2019 +0000
Revision:
4:d8dc8544fe81
Parent:
3:cb2db19c4076
Child:
5:63f5caea39bd
Fixes performance issues

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edmugu 0:7676da98b5c1 1 /* mbed Microcontroller Library
edmugu 0:7676da98b5c1 2 * Copyright (c) 2018 ARM Limited
edmugu 0:7676da98b5c1 3 * SPDX-License-Identifier: Apache-2.0
edmugu 0:7676da98b5c1 4 */
edmugu 0:7676da98b5c1 5
edmugu 0:7676da98b5c1 6 #include "mbed.h" // mbed OS 5.11
edmugu 0:7676da98b5c1 7 #include "Car.h" // DC motor driver
edmugu 0:7676da98b5c1 8 #include "IR_sensors.h" // ir sensors to track the line
edmugu 0:7676da98b5c1 9
edmugu 4:d8dc8544fe81 10 #define ADJ (1.0/12) //this is a time adjustment for bug in mbed OS 5.11
edmugu 0:7676da98b5c1 11 //#define ADJ (1.0/6) //this is a time adjustment for bug in mbed OS 5.11
edmugu 4:d8dc8544fe81 12 //#define ADJ (1.0) //this is a time adjustment for bug in mbed OS 5.11
edmugu 0:7676da98b5c1 13
edmugu 0:7676da98b5c1 14
edmugu 4:d8dc8544fe81 15 #define SPEED 0.1 // car speed
edmugu 0:7676da98b5c1 16 #define SLOWTURNCW 0.10 // speed to turn when the line still touching center
edmugu 0:7676da98b5c1 17 #define FASTTURNCW 0.50 // speed to turn when the line only touches side
edmugu 4:d8dc8544fe81 18 #define WAITMOTOR 0.01
edmugu 4:d8dc8544fe81 19 #define WAITSTATE 0.05
edmugu 0:7676da98b5c1 20
edmugu 0:7676da98b5c1 21 // the motors have the highest priority;
edmugu 0:7676da98b5c1 22 // this will be use to "track" its position
edmugu 0:7676da98b5c1 23 Thread threadMotors = Thread(osPriorityRealtime4, OS_STACK_SIZE, NULL, NULL);
edmugu 0:7676da98b5c1 24 Thread threadState = Thread(osPriorityRealtime1, OS_STACK_SIZE, NULL, NULL);
edmugu 0:7676da98b5c1 25
edmugu 1:2ceb12f88b5e 26 Queue<float,4> motor_turn_queue; // queue for motor turn commands;
edmugu 1:2ceb12f88b5e 27 Queue<float,4> motor_speed_queue; // queue for motor turn commands;
edmugu 0:7676da98b5c1 28 Car car(p21, p22, p23,p26, p25, p24); // controls two motors
edmugu 0:7676da98b5c1 29 IR_sensors ir(p18, p19, p20, LED1, LED2, LED3); // controls three IR sensors
edmugu 0:7676da98b5c1 30
edmugu 0:7676da98b5c1 31 float location = 0;
edmugu 0:7676da98b5c1 32
edmugu 0:7676da98b5c1 33 void motors() {
edmugu 4:d8dc8544fe81 34 car.speed(0.0);
edmugu 4:d8dc8544fe81 35 float t = 0.0;
edmugu 4:d8dc8544fe81 36 while (t < 3.0) {
edmugu 4:d8dc8544fe81 37 t += WAITMOTOR;
edmugu 4:d8dc8544fe81 38 //printf("motors::Checking Motors.. \t\tTime: %.2f \n\r", t);
edmugu 0:7676da98b5c1 39 location += car.speed();
edmugu 1:2ceb12f88b5e 40 if (not motor_turn_queue.empty()){
edmugu 1:2ceb12f88b5e 41 osEvent evt = motor_turn_queue.get();
edmugu 0:7676da98b5c1 42 if (evt.status == osEventMessage){
edmugu 0:7676da98b5c1 43 float *cmd = (float*)evt.value.p;
edmugu 4:d8dc8544fe81 44 //printf("motors::Motor turning: %.2f \n\r", *cmd);
edmugu 0:7676da98b5c1 45 car.turnCW(*cmd);
edmugu 0:7676da98b5c1 46 }
edmugu 0:7676da98b5c1 47 }
edmugu 1:2ceb12f88b5e 48 if (not motor_speed_queue.empty()){
edmugu 1:2ceb12f88b5e 49 osEvent evt = motor_speed_queue.get();
edmugu 1:2ceb12f88b5e 50 if (evt.status == osEventMessage){
edmugu 1:2ceb12f88b5e 51 float *cmd = (float*)evt.value.p;
edmugu 4:d8dc8544fe81 52 //printf("motors::Motor speeding: %.2f \n\r", *cmd);
edmugu 1:2ceb12f88b5e 53 car.speed(*cmd);
edmugu 1:2ceb12f88b5e 54 }
edmugu 1:2ceb12f88b5e 55 }
edmugu 4:d8dc8544fe81 56 wait(WAITMOTOR * ADJ);
edmugu 0:7676da98b5c1 57 }
edmugu 4:d8dc8544fe81 58 printf("motor is off");
edmugu 4:d8dc8544fe81 59 wait(WAITMOTOR * ADJ);
edmugu 4:d8dc8544fe81 60 car.speed(0.0);
edmugu 0:7676da98b5c1 61 }
edmugu 0:7676da98b5c1 62
edmugu 0:7676da98b5c1 63 void car_state() {
edmugu 1:2ceb12f88b5e 64 State prev_state = ir.state();
edmugu 4:d8dc8544fe81 65 float prev_speed = 0.0;
edmugu 4:d8dc8544fe81 66 wait(3* ADJ);
edmugu 0:7676da98b5c1 67 while (true) {
edmugu 0:7676da98b5c1 68 float turn;
edmugu 1:2ceb12f88b5e 69 float speed = SPEED;
edmugu 0:7676da98b5c1 70 switch (ir.state()) {
edmugu 0:7676da98b5c1 71 case left : turn = -FASTTURNCW;// turns left fast
edmugu 1:2ceb12f88b5e 72 printf("car_state::\t\t\tstate: Left\n\r");
edmugu 0:7676da98b5c1 73 break;
edmugu 0:7676da98b5c1 74 case centerleft : turn = -SLOWTURNCW; // turns left slow
edmugu 1:2ceb12f88b5e 75 printf("car_state::\t\t\tstate: Center Left\n\r");
edmugu 0:7676da98b5c1 76 break;
edmugu 0:7676da98b5c1 77 case center : turn = 0.0; // doesn't turn
edmugu 1:2ceb12f88b5e 78 printf("car_state::\t\t\tstate: Center\n\r");
edmugu 0:7676da98b5c1 79 break;
edmugu 0:7676da98b5c1 80 case centerright: turn = SLOWTURNCW; // turns right slow
edmugu 1:2ceb12f88b5e 81 printf("car_state::\t\t\tstate: Center Right\n\r");
edmugu 0:7676da98b5c1 82 break;
edmugu 0:7676da98b5c1 83 case right : turn = FASTTURNCW; // turns right fast
edmugu 1:2ceb12f88b5e 84 printf("car_state::\t\t\tstate: Right\n\r");
edmugu 0:7676da98b5c1 85 break;
edmugu 0:7676da98b5c1 86 default : turn = 0.0; // MAX turn right
edmugu 1:2ceb12f88b5e 87 speed = 0.0;
edmugu 1:2ceb12f88b5e 88 printf("car_state::\t\t\tstate: Default\n\r");
edmugu 1:2ceb12f88b5e 89 printf("car_state::Motor STOP command\n\r");
edmugu 0:7676da98b5c1 90 break;
edmugu 0:7676da98b5c1 91 }
edmugu 1:2ceb12f88b5e 92 if (prev_speed != speed) {
edmugu 1:2ceb12f88b5e 93 motor_speed_queue.put(&speed);
edmugu 1:2ceb12f88b5e 94 prev_speed = speed;
edmugu 1:2ceb12f88b5e 95 }
edmugu 1:2ceb12f88b5e 96 motor_turn_queue.put(&turn);
edmugu 4:d8dc8544fe81 97 //printf("car_state::Motor command: %.2f \n\r", turn);
edmugu 1:2ceb12f88b5e 98 prev_state = ir.state();
edmugu 4:d8dc8544fe81 99 wait(WAITSTATE * ADJ);
edmugu 0:7676da98b5c1 100 }
edmugu 0:7676da98b5c1 101 }
edmugu 0:7676da98b5c1 102
edmugu 0:7676da98b5c1 103 int main() {
edmugu 4:d8dc8544fe81 104 printf("PROGRAM STARTED");
edmugu 4:d8dc8544fe81 105 threadMotors.start(motors);
edmugu 4:d8dc8544fe81 106 threadState.start(car_state);
edmugu 0:7676da98b5c1 107 }