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@0:7676da98b5c1, 2019-03-26 (annotated)

Committer:
edmugu
Date:
Tue Mar 26 03:27:14 2019 +0000
Revision:
0:7676da98b5c1
Child:
1:2ceb12f88b5e
Initial Commit:; Basic functionality checks passed

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 0:7676da98b5c1 14 #define SPEED 0.001 // 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 0:7676da98b5c1 23 Queue<float,4> motor_queue; // queue for motor turn commands;
edmugu 0:7676da98b5c1 24 Car car(p21, p22, p23,p26, p25, p24); // controls two motors
edmugu 0:7676da98b5c1 25 IR_sensors ir(p18, p19, p20, LED1, LED2, LED3); // controls three IR sensors
edmugu 0:7676da98b5c1 26
edmugu 0:7676da98b5c1 27 float location = 0;
edmugu 0:7676da98b5c1 28
edmugu 0:7676da98b5c1 29 void motors() {
edmugu 0:7676da98b5c1 30 while (true) {
edmugu 0:7676da98b5c1 31 printf("Checking Motors.. \n\r");
edmugu 0:7676da98b5c1 32 location += car.speed();
edmugu 0:7676da98b5c1 33 car.speed(SPEED);
edmugu 0:7676da98b5c1 34 if (not motor_queue.empty()){
edmugu 0:7676da98b5c1 35 osEvent evt = motor_queue.get();
edmugu 0:7676da98b5c1 36 if (evt.status == osEventMessage){
edmugu 0:7676da98b5c1 37 float *cmd = (float*)evt.value.p;
edmugu 0:7676da98b5c1 38 printf("Motor turning: %.2f \n\r", *cmd);
edmugu 0:7676da98b5c1 39 car.turnCW(*cmd);
edmugu 0:7676da98b5c1 40 }
edmugu 0:7676da98b5c1 41 }
edmugu 0:7676da98b5c1 42 wait(0.20 * ADJ);
edmugu 0:7676da98b5c1 43 }
edmugu 0:7676da98b5c1 44 }
edmugu 0:7676da98b5c1 45
edmugu 0:7676da98b5c1 46 void car_state() {
edmugu 0:7676da98b5c1 47 State prevState = ir.state();
edmugu 0:7676da98b5c1 48 while (true) {
edmugu 0:7676da98b5c1 49 float turn;
edmugu 0:7676da98b5c1 50 switch (ir.state()) {
edmugu 0:7676da98b5c1 51 case left : turn = -FASTTURNCW;// turns left fast
edmugu 0:7676da98b5c1 52 printf("\t\t\tstate: Left\n\r");
edmugu 0:7676da98b5c1 53 break;
edmugu 0:7676da98b5c1 54 case centerleft : turn = -SLOWTURNCW; // turns left slow
edmugu 0:7676da98b5c1 55 printf("\t\t\tstate: Center Left\n\r");
edmugu 0:7676da98b5c1 56 break;
edmugu 0:7676da98b5c1 57 case center : turn = 0.0; // doesn't turn
edmugu 0:7676da98b5c1 58 printf("\t\t\tstate: Center\n\r");
edmugu 0:7676da98b5c1 59 break;
edmugu 0:7676da98b5c1 60 case centerright: turn = SLOWTURNCW; // turns right slow
edmugu 0:7676da98b5c1 61 printf("\t\t\tstate: Center Right\n\r");
edmugu 0:7676da98b5c1 62 break;
edmugu 0:7676da98b5c1 63 case right : turn = FASTTURNCW; // turns right fast
edmugu 0:7676da98b5c1 64 printf("\t\t\tstate: Right\n\r");
edmugu 0:7676da98b5c1 65 break;
edmugu 0:7676da98b5c1 66 default : turn = 0.0; // MAX turn right
edmugu 0:7676da98b5c1 67 printf("\t\t\tstate: Default\n\r");
edmugu 0:7676da98b5c1 68 break;
edmugu 0:7676da98b5c1 69 }
edmugu 0:7676da98b5c1 70 motor_queue.put(&turn);
edmugu 0:7676da98b5c1 71 printf("Motor command: %.2f \n\r", turn);
edmugu 0:7676da98b5c1 72 prevState = ir.state();
edmugu 0:7676da98b5c1 73 wait(1.0 * ADJ);
edmugu 0:7676da98b5c1 74 }
edmugu 0:7676da98b5c1 75 }
edmugu 0:7676da98b5c1 76
edmugu 0:7676da98b5c1 77 int main() {
edmugu 0:7676da98b5c1 78 printf("PROGRAM STARTED");
edmugu 0:7676da98b5c1 79 threadMotors.start(motors);
edmugu 0:7676da98b5c1 80 threadState.start(car_state);
edmugu 0:7676da98b5c1 81 }