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@1:2ceb12f88b5e, 2019-03-26 (annotated)

Committer:
edmugu
Date:
Tue Mar 26 03:58:41 2019 +0000
Revision:
1:2ceb12f88b5e
Parent:
0:7676da98b5c1
Child:
2:49fbdf1e7fb0
Adds speed queue for the motor thread

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