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@7:d16faa6d7713, 2019-04-10 (annotated)

Committer:
edmugu
Date:
Wed Apr 10 02:16:32 2019 +0000
Revision:
7:d16faa6d7713
Parent:
5:63f5caea39bd
Child:
8:14e91fdf70e8
Final Presentable

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 7:d16faa6d7713 6 /*******************************************************************************
edmugu 7:d16faa6d7713 7 * DEFENITIONS
edmugu 7:d16faa6d7713 8 *
edmugu 7:d16faa6d7713 9 *******************************************************************************/
edmugu 7:d16faa6d7713 10
edmugu 7:d16faa6d7713 11
edmugu 5:63f5caea39bd 12 #include "mbed.h" // mbed OS 5.11
edmugu 5:63f5caea39bd 13 #include "Car.h" // DC motor driver
edmugu 5:63f5caea39bd 14 #include "IR_sensors.h" // ir sensors to track the line
edmugu 0:7676da98b5c1 15
edmugu 7:d16faa6d7713 16 #define DEBUG 0 // enables the print statements
edmugu 7:d16faa6d7713 17 #define ADJ (0.1) // this is a time adjustment for bug in mbed OS 5.11
edmugu 5:63f5caea39bd 18 //#define ADJ (1.0/6) // this is a time adjustment for bug in mbed OS 5.11
edmugu 5:63f5caea39bd 19 //#define ADJ (1.0) // this is a time adjustment for bug in mbed OS 5.11
edmugu 0:7676da98b5c1 20
edmugu 0:7676da98b5c1 21
edmugu 5:63f5caea39bd 22 #define TOTALTIME 100.0
edmugu 5:63f5caea39bd 23 #define STEP 0.1 // each motor step is half a second
edmugu 5:63f5caea39bd 24 #define SPEED 1.0 // car speed
edmugu 7:d16faa6d7713 25 #define SLOWTURNCW -0.90 // speed to turn when the line still touching center
edmugu 7:d16faa6d7713 26 #define FASTTURNCW -1.00 // speed to turn when the line only touches side
edmugu 4:d8dc8544fe81 27 #define WAITMOTOR 0.01
edmugu 4:d8dc8544fe81 28 #define WAITSTATE 0.05
edmugu 0:7676da98b5c1 29
edmugu 0:7676da98b5c1 30 // the motors have the highest priority;
edmugu 0:7676da98b5c1 31 // this will be use to "track" its position
edmugu 0:7676da98b5c1 32 Thread threadMotors = Thread(osPriorityRealtime4, OS_STACK_SIZE, NULL, NULL);
edmugu 0:7676da98b5c1 33 Thread threadState = Thread(osPriorityRealtime1, OS_STACK_SIZE, NULL, NULL);
edmugu 0:7676da98b5c1 34
edmugu 1:2ceb12f88b5e 35 Queue<float,4> motor_turn_queue; // queue for motor turn commands;
edmugu 1:2ceb12f88b5e 36 Queue<float,4> motor_speed_queue; // queue for motor turn commands;
edmugu 0:7676da98b5c1 37 Car car(p21, p22, p23,p26, p25, p24); // controls two motors
edmugu 0:7676da98b5c1 38 IR_sensors ir(p18, p19, p20, LED1, LED2, LED3); // controls three IR sensors
edmugu 0:7676da98b5c1 39
edmugu 0:7676da98b5c1 40 float location = 0;
edmugu 0:7676da98b5c1 41
edmugu 7:d16faa6d7713 42
edmugu 7:d16faa6d7713 43
edmugu 7:d16faa6d7713 44 /*******************************************************************************
edmugu 7:d16faa6d7713 45 * MOTOR FUNCTION
edmugu 7:d16faa6d7713 46 *
edmugu 7:d16faa6d7713 47 *******************************************************************************/
edmugu 7:d16faa6d7713 48
edmugu 0:7676da98b5c1 49 void motors() {
edmugu 5:63f5caea39bd 50 //
edmugu 5:63f5caea39bd 51 // It checks the turn and speed queues and takes repective action.
edmugu 5:63f5caea39bd 52 //
edmugu 5:63f5caea39bd 53 car.speed(0.0);
edmugu 5:63f5caea39bd 54 float total_time = 0.0;
edmugu 5:63f5caea39bd 55 float step_time = 0.0; // this will be to control the DC motor step
edmugu 5:63f5caea39bd 56 int step_time_on = 0;
edmugu 5:63f5caea39bd 57 float current_speed = 0.0;
edmugu 5:63f5caea39bd 58 while (total_time < TOTALTIME) {
edmugu 5:63f5caea39bd 59 total_time += WAITMOTOR;
edmugu 5:63f5caea39bd 60 //if (DEBUG)
edmugu 5:63f5caea39bd 61 // printf("motors::Checking Motors.. \t\tTime: %.2f \n\r", total_time);
edmugu 0:7676da98b5c1 62 location += car.speed();
edmugu 5:63f5caea39bd 63
edmugu 5:63f5caea39bd 64 // TURN ROUTINE
edmugu 1:2ceb12f88b5e 65 if (not motor_turn_queue.empty()){
edmugu 1:2ceb12f88b5e 66 osEvent evt = motor_turn_queue.get();
edmugu 0:7676da98b5c1 67 if (evt.status == osEventMessage){
edmugu 0:7676da98b5c1 68 float *cmd = (float*)evt.value.p;
edmugu 5:63f5caea39bd 69 //if (DEBUG)
edmugu 5:63f5caea39bd 70 // printf("motors::Motor turning: %.2f \n\r", *cmd);
edmugu 0:7676da98b5c1 71 car.turnCW(*cmd);
edmugu 0:7676da98b5c1 72 }
edmugu 0:7676da98b5c1 73 }
edmugu 5:63f5caea39bd 74
edmugu 5:63f5caea39bd 75 // SPEED ROUTINE
edmugu 1:2ceb12f88b5e 76 if (not motor_speed_queue.empty()){
edmugu 1:2ceb12f88b5e 77 osEvent evt = motor_speed_queue.get();
edmugu 5:63f5caea39bd 78 step_time = total_time + STEP;
edmugu 1:2ceb12f88b5e 79 if (evt.status == osEventMessage){
edmugu 1:2ceb12f88b5e 80 float *cmd = (float*)evt.value.p;
edmugu 5:63f5caea39bd 81 //if (DEBUG)
edmugu 5:63f5caea39bd 82 // printf("motors::Motor speeding: %.2f \n\r", *cmd);
edmugu 5:63f5caea39bd 83 current_speed = *cmd;
edmugu 1:2ceb12f88b5e 84 car.speed(*cmd);
edmugu 1:2ceb12f88b5e 85 }
edmugu 1:2ceb12f88b5e 86 }
edmugu 5:63f5caea39bd 87
edmugu 5:63f5caea39bd 88 if (total_time > step_time) {
edmugu 5:63f5caea39bd 89 step_time = total_time + STEP;
edmugu 5:63f5caea39bd 90 step_time_on = ~ step_time_on;
edmugu 5:63f5caea39bd 91 //if (DEBUG)
edmugu 5:63f5caea39bd 92 // printf("\n\rmotor stop\n\r");
edmugu 5:63f5caea39bd 93 if (step_time_on)
edmugu 5:63f5caea39bd 94 car.speed(current_speed);
edmugu 5:63f5caea39bd 95 else
edmugu 5:63f5caea39bd 96 car.speed(0.0);
edmugu 5:63f5caea39bd 97 }
edmugu 4:d8dc8544fe81 98 wait(WAITMOTOR * ADJ);
edmugu 0:7676da98b5c1 99 }
edmugu 5:63f5caea39bd 100 if (DEBUG)
edmugu 4:d8dc8544fe81 101 printf("motor is off");
edmugu 5:63f5caea39bd 102 wait(WAITMOTOR * ADJ);
edmugu 5:63f5caea39bd 103 car.speed(0.0);
edmugu 0:7676da98b5c1 104 }
edmugu 0:7676da98b5c1 105
edmugu 7:d16faa6d7713 106
edmugu 7:d16faa6d7713 107
edmugu 7:d16faa6d7713 108 /*******************************************************************************
edmugu 7:d16faa6d7713 109 * STATE MACHINE
edmugu 7:d16faa6d7713 110 *
edmugu 7:d16faa6d7713 111 *******************************************************************************/
edmugu 7:d16faa6d7713 112
edmugu 0:7676da98b5c1 113 void car_state() {
edmugu 4:d8dc8544fe81 114 float prev_speed = 0.0;
edmugu 5:63f5caea39bd 115 float prev_turn = 0.0;
edmugu 5:63f5caea39bd 116 float speed = SPEED;
edmugu 5:63f5caea39bd 117 float turn;
edmugu 5:63f5caea39bd 118 int stateint = -1;
edmugu 5:63f5caea39bd 119 int prev_stateint = -1;
edmugu 5:63f5caea39bd 120 State prev_state = undef0;
edmugu 5:63f5caea39bd 121 wait(3* ADJ); // wait before you move
edmugu 0:7676da98b5c1 122 while (true) {
edmugu 5:63f5caea39bd 123 State switchstate = ir.state();
edmugu 5:63f5caea39bd 124 if ((switchstate == undef0) and (prev_state != undef0))
edmugu 5:63f5caea39bd 125 switchstate = prev_state;
edmugu 5:63f5caea39bd 126 switch (switchstate) {
edmugu 0:7676da98b5c1 127 case left : turn = -FASTTURNCW;// turns left fast
edmugu 5:63f5caea39bd 128 speed = SPEED / 2;
edmugu 5:63f5caea39bd 129 if (DEBUG)
edmugu 5:63f5caea39bd 130 printf("car_state::\t\t\tstate: Left\n\r");
edmugu 0:7676da98b5c1 131 break;
edmugu 5:63f5caea39bd 132 case centerleft : turn = -SLOWTURNCW; // turns left slow
edmugu 5:63f5caea39bd 133 speed = SPEED / 2;
edmugu 5:63f5caea39bd 134 if (DEBUG)
edmugu 5:63f5caea39bd 135 printf("car_state::\t\t\tstate: Center Left\n\r");
edmugu 0:7676da98b5c1 136 break;
edmugu 0:7676da98b5c1 137 case center : turn = 0.0; // doesn't turn
edmugu 5:63f5caea39bd 138 speed = SPEED;
edmugu 5:63f5caea39bd 139 if (DEBUG)
edmugu 5:63f5caea39bd 140 printf("car_state::\t\t\tstate: Center\n\r");
edmugu 0:7676da98b5c1 141 break;
edmugu 0:7676da98b5c1 142 case centerright: turn = SLOWTURNCW; // turns right slow
edmugu 5:63f5caea39bd 143 speed = SPEED / 2;
edmugu 5:63f5caea39bd 144 if (DEBUG)
edmugu 5:63f5caea39bd 145 printf("car_state::\t\t\tstate: Center Right\n\r");
edmugu 0:7676da98b5c1 146 break;
edmugu 0:7676da98b5c1 147 case right : turn = FASTTURNCW; // turns right fast
edmugu 5:63f5caea39bd 148 speed = SPEED / 2;
edmugu 5:63f5caea39bd 149 if (DEBUG)
edmugu 5:63f5caea39bd 150 printf("car_state::\t\t\tstate: Right\n\r");
edmugu 0:7676da98b5c1 151 break;
edmugu 0:7676da98b5c1 152 default : turn = 0.0; // MAX turn right
edmugu 1:2ceb12f88b5e 153 speed = 0.0;
edmugu 5:63f5caea39bd 154 if (DEBUG)
edmugu 5:63f5caea39bd 155 printf("car_state::\t\t\tstate: Default\n\r");
edmugu 0:7676da98b5c1 156 break;
edmugu 0:7676da98b5c1 157 }
edmugu 5:63f5caea39bd 158 if ((prev_speed != speed) or (prev_turn != turn)) {
edmugu 5:63f5caea39bd 159 printf("car_state::\t\tchanging speed: Default\n\r");
edmugu 5:63f5caea39bd 160 float temp = 0.0;
edmugu 5:63f5caea39bd 161 motor_speed_queue.put(&temp);
edmugu 5:63f5caea39bd 162 wait(10 * WAITSTATE * ADJ);
edmugu 5:63f5caea39bd 163 printf("car_state::\t\tchanging speed: Default\n\r");
edmugu 1:2ceb12f88b5e 164 motor_speed_queue.put(&speed);
edmugu 1:2ceb12f88b5e 165 prev_speed = speed;
edmugu 5:63f5caea39bd 166 prev_turn = turn;
edmugu 1:2ceb12f88b5e 167 }
edmugu 1:2ceb12f88b5e 168 motor_turn_queue.put(&turn);
edmugu 7:d16faa6d7713 169 prev_state = ir.state();
edmugu 7:d16faa6d7713 170 wait(WAITSTATE * ADJ);
edmugu 7:d16faa6d7713 171 if (switchstate != undef0)
edmugu 5:63f5caea39bd 172 prev_state = ir.state();
edmugu 0:7676da98b5c1 173 }
edmugu 0:7676da98b5c1 174 }
edmugu 5:63f5caea39bd 175
edmugu 5:63f5caea39bd 176
edmugu 7:d16faa6d7713 177 /*******************************************************************************
edmugu 7:d16faa6d7713 178 * START THREADS
edmugu 7:d16faa6d7713 179 *
edmugu 7:d16faa6d7713 180 *******************************************************************************/
edmugu 7:d16faa6d7713 181
edmugu 7:d16faa6d7713 182
edmugu 0:7676da98b5c1 183 int main() {
edmugu 5:63f5caea39bd 184 printf("\n\rPROGRAM STARTED\n\r");
edmugu 4:d8dc8544fe81 185 threadMotors.start(motors);
edmugu 4:d8dc8544fe81 186 threadState.start(car_state);
edmugu 0:7676da98b5c1 187 }