Boa-Lin Lai / Mbed 2 deprecated robot

Dependencies:   Ble Motor mbed

main.cpp@1:26b2b1545e13, 2016-11-04 (annotated)

Committer:
ece4180fl16
Date:
Fri Nov 04 15:52:58 2016 +0000
Revision:
1:26b2b1545e13
Parent:
0:609a09b59dc5 
ece4180 demo; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ece4180fl16 0:609a09b59dc5 1 /*
ece4180fl16 0:609a09b59dc5 2 ECE 4180 GATECH Fall 2016
ece4180fl16 0:609a09b59dc5 3 Boa-Lin Lai, Sameual Choi
ece4180fl16 0:609a09b59dc5 4 */
ece4180fl16 0:609a09b59dc5 5 #include "mbed.h"
ece4180fl16 0:609a09b59dc5 6 #include "Motor.h"
ece4180fl16 0:609a09b59dc5 7 #include "ble.h"
ece4180fl16 0:609a09b59dc5 8 #include <stdlib.h>
ece4180fl16 0:609a09b59dc5 9 #include <algorithm>
ece4180fl16 0:609a09b59dc5 10 DigitalOut myled1(LED1);
ece4180fl16 0:609a09b59dc5 11 DigitalOut myled2(LED2);
ece4180fl16 0:609a09b59dc5 12 DigitalOut myled3(LED3);
ece4180fl16 0:609a09b59dc5 13 DigitalOut myled4(LED4);
ece4180fl16 0:609a09b59dc5 14 AnalogIn wallSensor(p19);
ece4180fl16 0:609a09b59dc5 15 AnalogIn edgeSensor(p20);
ece4180fl16 0:609a09b59dc5 16 PwmOut speaker(p21);
ece4180fl16 0:609a09b59dc5 17 Serial pc(USBTX, USBRX);
ece4180fl16 0:609a09b59dc5 18 Motor ml(p23, p12, p11); // pwm, fwd, rev
ece4180fl16 0:609a09b59dc5 19 Motor mr(p24, p14, p13); // pwm, fwd, rev
ece4180fl16 0:609a09b59dc5 20 //Interrupt routine to parse message with one new character per serial RX interrupt
ece4180fl16 0:609a09b59dc5 21 volatile bool isOn = false;
ece4180fl16 0:609a09b59dc5 22 volatile bool edge = false;
ece4180fl16 0:609a09b59dc5 23 volatile bool wall = false;
ece4180fl16 0:609a09b59dc5 24 volatile double speed = 0.0;
ece4180fl16 0:609a09b59dc5 25 volatile double wallDis = 0.0;
ece4180fl16 0:609a09b59dc5 26 volatile double edgeDis = 0.0;
ece4180fl16 0:609a09b59dc5 27 Ticker edge_detector;
ece4180fl16 0:609a09b59dc5 28 Ticker wall_detector;
ece4180fl16 0:609a09b59dc5 29
ece4180fl16 0:609a09b59dc5 30 void edgeMove()
ece4180fl16 0:609a09b59dc5 31 {
ece4180fl16 0:609a09b59dc5 32 ml.speed(-0.3);
ece4180fl16 0:609a09b59dc5 33 mr.speed(-0.3);
ece4180fl16 0:609a09b59dc5 34 for (int i=0; i<10; i=i+2) {
ece4180fl16 0:609a09b59dc5 35 speaker.period(1.0/969.0);
ece4180fl16 0:609a09b59dc5 36 speaker = float(i)/50.0;
ece4180fl16 0:609a09b59dc5 37 wait(.25);
ece4180fl16 0:609a09b59dc5 38 speaker.period(1.0/800.0);
ece4180fl16 0:609a09b59dc5 39 wait(.25);
ece4180fl16 0:609a09b59dc5 40 }
ece4180fl16 0:609a09b59dc5 41 edge = false;
ece4180fl16 0:609a09b59dc5 42 wall = false;
ece4180fl16 0:609a09b59dc5 43 speaker = 0;
ece4180fl16 0:609a09b59dc5 44 ml.speed(-0.5);
ece4180fl16 0:609a09b59dc5 45 mr.speed(0.5);
ece4180fl16 0:609a09b59dc5 46 wait(0.5);
ece4180fl16 0:609a09b59dc5 47 }
ece4180fl16 0:609a09b59dc5 48 void isEdge() {
ece4180fl16 0:609a09b59dc5 49
ece4180fl16 0:609a09b59dc5 50 if(edgeDis > 2.5)
ece4180fl16 0:609a09b59dc5 51 {
ece4180fl16 0:609a09b59dc5 52 speed = 0;
ece4180fl16 0:609a09b59dc5 53 edge = true;
ece4180fl16 0:609a09b59dc5 54 }
ece4180fl16 0:609a09b59dc5 55 else
ece4180fl16 0:609a09b59dc5 56 {
ece4180fl16 0:609a09b59dc5 57 edge = false;
ece4180fl16 0:609a09b59dc5 58 }
ece4180fl16 0:609a09b59dc5 59 }
ece4180fl16 0:609a09b59dc5 60
ece4180fl16 0:609a09b59dc5 61 void isWall() {
ece4180fl16 0:609a09b59dc5 62
ece4180fl16 0:609a09b59dc5 63 if(wallDis > 0.7)
ece4180fl16 0:609a09b59dc5 64 {
ece4180fl16 0:609a09b59dc5 65 speed = 0;
ece4180fl16 0:609a09b59dc5 66 wall = true;
ece4180fl16 0:609a09b59dc5 67 }
ece4180fl16 0:609a09b59dc5 68 else
ece4180fl16 0:609a09b59dc5 69 {
ece4180fl16 0:609a09b59dc5 70 wall = false;
ece4180fl16 0:609a09b59dc5 71 }
ece4180fl16 0:609a09b59dc5 72 }
ece4180fl16 0:609a09b59dc5 73
ece4180fl16 0:609a09b59dc5 74
ece4180fl16 0:609a09b59dc5 75 void BLE_action()
ece4180fl16 0:609a09b59dc5 76 {
ece4180fl16 0:609a09b59dc5 77 if(button_ready && (bnum=='1')) { // button 4 changed
ece4180fl16 0:609a09b59dc5 78 if(isOn == true)
ece4180fl16 0:609a09b59dc5 79 {
ece4180fl16 0:609a09b59dc5 80 speed += 0.1;
ece4180fl16 0:609a09b59dc5 81 if(speed > 0.7) speed = 0.7;
ece4180fl16 0:609a09b59dc5 82 }
ece4180fl16 0:609a09b59dc5 83 myled1 = bhit - '0'; //turn led4 on/off
ece4180fl16 0:609a09b59dc5 84 button_ready = 0; //reset flag after reading button message
ece4180fl16 0:609a09b59dc5 85 }
ece4180fl16 0:609a09b59dc5 86 else if(button_ready && (bnum=='2')) { // button 4 changed
ece4180fl16 0:609a09b59dc5 87 if(isOn == true)
ece4180fl16 0:609a09b59dc5 88 {
ece4180fl16 0:609a09b59dc5 89 speed -= 0.1;
ece4180fl16 0:609a09b59dc5 90 if(speed < 0.3 ) speed = 0.3;
ece4180fl16 0:609a09b59dc5 91 }
ece4180fl16 0:609a09b59dc5 92 myled2 = bhit - '0'; //turn led4 on/off
ece4180fl16 0:609a09b59dc5 93 button_ready = 0; //reset flag after reading button message
ece4180fl16 0:609a09b59dc5 94 }
ece4180fl16 0:609a09b59dc5 95 else if(button_ready && (bnum=='3')) { // button 4 changed
ece4180fl16 0:609a09b59dc5 96 speed = 0;
ece4180fl16 0:609a09b59dc5 97 isOn = false;
ece4180fl16 0:609a09b59dc5 98 myled3 = bhit - '0'; //turn led4 on/off
ece4180fl16 0:609a09b59dc5 99 button_ready = 0; //reset flag after reading button message
ece4180fl16 0:609a09b59dc5 100 }
ece4180fl16 0:609a09b59dc5 101 else if(button_ready && (bnum=='4')) { // button 4 changed
ece4180fl16 0:609a09b59dc5 102 isOn = true;
ece4180fl16 0:609a09b59dc5 103 speed = 0.3;
ece4180fl16 0:609a09b59dc5 104 myled4 = bhit - '0'; //turn led4 on/off
ece4180fl16 0:609a09b59dc5 105 button_ready = 0; //reset flag after reading button message
ece4180fl16 0:609a09b59dc5 106 }
ece4180fl16 0:609a09b59dc5 107
ece4180fl16 0:609a09b59dc5 108
ece4180fl16 0:609a09b59dc5 109
ece4180fl16 0:609a09b59dc5 110 }
ece4180fl16 0:609a09b59dc5 111 void print_speed()
ece4180fl16 0:609a09b59dc5 112 {
ece4180fl16 0:609a09b59dc5 113 pc.printf("%-76s%0.2f\n",speed);
ece4180fl16 0:609a09b59dc5 114 }
ece4180fl16 0:609a09b59dc5 115 int main()
ece4180fl16 0:609a09b59dc5 116 {
ece4180fl16 0:609a09b59dc5 117 //attach interrupt function for each new Bluetooth serial character
ece4180fl16 0:609a09b59dc5 118 Blue.attach(&parse_message,Serial::RxIrq);
ece4180fl16 0:609a09b59dc5 119 edge_detector.attach(&isEdge, 0.02);
ece4180fl16 0:609a09b59dc5 120 wall_detector.attach(&isWall , 0.02);
ece4180fl16 0:609a09b59dc5 121 while(1) {
ece4180fl16 0:609a09b59dc5 122 //check for a new button message ready
ece4180fl16 0:609a09b59dc5 123 BLE_action();
ece4180fl16 0:609a09b59dc5 124 //do other tasks in main - interrupts will process button message characters
ece4180fl16 0:609a09b59dc5 125 myled4 = 1;
ece4180fl16 0:609a09b59dc5 126 wallDis = wallSensor;
ece4180fl16 0:609a09b59dc5 127 edgeDis = 1/edgeSensor;
ece4180fl16 0:609a09b59dc5 128 wait(0.05);
ece4180fl16 0:609a09b59dc5 129 myled4 = 0;
ece4180fl16 0:609a09b59dc5 130 wait(0.05);
ece4180fl16 0:609a09b59dc5 131 ml.speed(speed);
ece4180fl16 0:609a09b59dc5 132 mr.speed(pow(speed,0.2));
ece4180fl16 0:609a09b59dc5 133 if((edge == true) || (wall == true))
ece4180fl16 0:609a09b59dc5 134 {
ece4180fl16 0:609a09b59dc5 135 edgeMove();
ece4180fl16 0:609a09b59dc5 136 }
ece4180fl16 0:609a09b59dc5 137 //pc.printf("IR sensor reads %2.2f\n ", a);
ece4180fl16 0:609a09b59dc5 138 //pc.printf("\rDistance is %2.2f cm \n ", a);
ece4180fl16 0:609a09b59dc5 139 //pc.printf("%0.5f\n",dis);
ece4180fl16 0:609a09b59dc5 140 }
ece4180fl16 0:609a09b59dc5 141 }