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@5:63f5caea39bd, 2019-03-28 (annotated)

Committer:
edmugu
Date:
Thu Mar 28 16:10:50 2019 +0000
Revision:
5:63f5caea39bd
Parent:
4:d8dc8544fe81
Child:
7:d16faa6d7713
Presentation Ready;

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