Kshitij Kulkarni / Mbed 2 deprecated flamebot

ECE 4180 FlameBot Source Code

Dependencies:   HC_SR04_Ultrasonic_Library Motor Servo mbed

main.cpp@0:7d7387eefc5a, 2018-05-02 (annotated)

Committer:
kkulkarni36
Date:
Wed May 02 04:26:40 2018 +0000
Revision:
0:7d7387eefc5a
ECE 4180 Flamebot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kkulkarni36 0:7d7387eefc5a 1 #include "mbed.h"
kkulkarni36 0:7d7387eefc5a 2 #include "Motor.h"
kkulkarni36 0:7d7387eefc5a 3 #include "Servo.h"
kkulkarni36 0:7d7387eefc5a 4 #include "ultrasonic.h"
kkulkarni36 0:7d7387eefc5a 5
kkulkarni36 0:7d7387eefc5a 6 Motor m1(p25, p15, p14); // pwm, fwd, rev
kkulkarni36 0:7d7387eefc5a 7 Motor m2(p26, p19, p20 ); // pwm, fwd, rev
kkulkarni36 0:7d7387eefc5a 8 volatile bool move = 1;
kkulkarni36 0:7d7387eefc5a 9 Serial bluemod(p9,p10);
kkulkarni36 0:7d7387eefc5a 10 Serial pc(USBTX,USBRX);
kkulkarni36 0:7d7387eefc5a 11 Servo myservo(p24);
kkulkarni36 0:7d7387eefc5a 12 DigitalOut myled1(LED1);
kkulkarni36 0:7d7387eefc5a 13 DigitalOut myled2(LED2);
kkulkarni36 0:7d7387eefc5a 14 DigitalOut myled3(LED3);
kkulkarni36 0:7d7387eefc5a 15 DigitalOut myled4(LED4);
kkulkarni36 0:7d7387eefc5a 16
kkulkarni36 0:7d7387eefc5a 17
kkulkarni36 0:7d7387eefc5a 18
kkulkarni36 0:7d7387eefc5a 19 //C union can convert 4 chars to a float - puts them in same location in memory
kkulkarni36 0:7d7387eefc5a 20 //trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float
kkulkarni36 0:7d7387eefc5a 21 union f_or_char {
kkulkarni36 0:7d7387eefc5a 22 float f;
kkulkarni36 0:7d7387eefc5a 23 char c[4];
kkulkarni36 0:7d7387eefc5a 24 };
kkulkarni36 0:7d7387eefc5a 25
kkulkarni36 0:7d7387eefc5a 26 void dist(int distance)
kkulkarni36 0:7d7387eefc5a 27 {
kkulkarni36 0:7d7387eefc5a 28 if (distance < 500.0) {
kkulkarni36 0:7d7387eefc5a 29 m1.speed(0.0);
kkulkarni36 0:7d7387eefc5a 30 m2.speed(0.0);
kkulkarni36 0:7d7387eefc5a 31 wait(1);
kkulkarni36 0:7d7387eefc5a 32 myservo = 0.0;
kkulkarni36 0:7d7387eefc5a 33 wait(1);
kkulkarni36 0:7d7387eefc5a 34 m1.speed(0.3);
kkulkarni36 0:7d7387eefc5a 35 m2.speed(-0.3);
kkulkarni36 0:7d7387eefc5a 36 wait(1.5);
kkulkarni36 0:7d7387eefc5a 37 m1.speed(0.0);
kkulkarni36 0:7d7387eefc5a 38 m2.speed(0.0);
kkulkarni36 0:7d7387eefc5a 39 wait(1.5);
kkulkarni36 0:7d7387eefc5a 40 move = 0;
kkulkarni36 0:7d7387eefc5a 41 myservo = 1.0;
kkulkarni36 0:7d7387eefc5a 42 myled1 = 1;
kkulkarni36 0:7d7387eefc5a 43 myled2 = 1;
kkulkarni36 0:7d7387eefc5a 44 myled3 = 1;
kkulkarni36 0:7d7387eefc5a 45 myled4 = 1;
kkulkarni36 0:7d7387eefc5a 46 wait(1);
kkulkarni36 0:7d7387eefc5a 47 myservo = 0.0;
kkulkarni36 0:7d7387eefc5a 48 myled1 = 0;
kkulkarni36 0:7d7387eefc5a 49 myled2 = 0;
kkulkarni36 0:7d7387eefc5a 50 myled3 = 0;
kkulkarni36 0:7d7387eefc5a 51 myled4 = 0;
kkulkarni36 0:7d7387eefc5a 52 wait(1);
kkulkarni36 0:7d7387eefc5a 53 }
kkulkarni36 0:7d7387eefc5a 54 }
kkulkarni36 0:7d7387eefc5a 55
kkulkarni36 0:7d7387eefc5a 56 ultrasonic sonar(p6, p7, .1, 1, &dist);
kkulkarni36 0:7d7387eefc5a 57
kkulkarni36 0:7d7387eefc5a 58 int main()
kkulkarni36 0:7d7387eefc5a 59 {
kkulkarni36 0:7d7387eefc5a 60 float s1;
kkulkarni36 0:7d7387eefc5a 61 float s2;
kkulkarni36 0:7d7387eefc5a 62 char bchecksum=0;
kkulkarni36 0:7d7387eefc5a 63 char temp=0;
kkulkarni36 0:7d7387eefc5a 64 union f_or_char x,y,z;
kkulkarni36 0:7d7387eefc5a 65
kkulkarni36 0:7d7387eefc5a 66 sonar.startUpdates();//start measuring the distance
kkulkarni36 0:7d7387eefc5a 67
kkulkarni36 0:7d7387eefc5a 68 while(1) {
kkulkarni36 0:7d7387eefc5a 69 bchecksum=0;
kkulkarni36 0:7d7387eefc5a 70 if (bluemod.getc()=='!') {
kkulkarni36 0:7d7387eefc5a 71 if (bluemod.getc()=='A') { //Accelerometer data packet
kkulkarni36 0:7d7387eefc5a 72 for (int i=0; i<4; i++) {
kkulkarni36 0:7d7387eefc5a 73 temp = bluemod.getc();
kkulkarni36 0:7d7387eefc5a 74 x.c[i] = temp;
kkulkarni36 0:7d7387eefc5a 75 bchecksum = bchecksum + temp;
kkulkarni36 0:7d7387eefc5a 76 }
kkulkarni36 0:7d7387eefc5a 77 for (int i=0; i<4; i++) {
kkulkarni36 0:7d7387eefc5a 78 temp = bluemod.getc();
kkulkarni36 0:7d7387eefc5a 79 y.c[i] = temp;
kkulkarni36 0:7d7387eefc5a 80 bchecksum = bchecksum + temp;
kkulkarni36 0:7d7387eefc5a 81 }
kkulkarni36 0:7d7387eefc5a 82 for (int i=0; i<4; i++) {
kkulkarni36 0:7d7387eefc5a 83 temp = bluemod.getc();
kkulkarni36 0:7d7387eefc5a 84 z.c[i] = temp;
kkulkarni36 0:7d7387eefc5a 85 bchecksum = bchecksum + temp;
kkulkarni36 0:7d7387eefc5a 86 }
kkulkarni36 0:7d7387eefc5a 87 if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK?
kkulkarni36 0:7d7387eefc5a 88 //pc.printf("X = %f Y = %f Z = %f\n\r",x.f, y.f, z.f);
kkulkarni36 0:7d7387eefc5a 89 move = 1;
kkulkarni36 0:7d7387eefc5a 90 if (x.f < -4.0) {
kkulkarni36 0:7d7387eefc5a 91 s1 = 0.3;
kkulkarni36 0:7d7387eefc5a 92 s2 = -0.3;
kkulkarni36 0:7d7387eefc5a 93 m1.speed(s1);
kkulkarni36 0:7d7387eefc5a 94 m2.speed(s2);
kkulkarni36 0:7d7387eefc5a 95 myled1 = 1;
kkulkarni36 0:7d7387eefc5a 96 myled2 = 0;
kkulkarni36 0:7d7387eefc5a 97 myled3 = 0;
kkulkarni36 0:7d7387eefc5a 98 myled4 = 0;
kkulkarni36 0:7d7387eefc5a 99 wait(0.05);
kkulkarni36 0:7d7387eefc5a 100 }
kkulkarni36 0:7d7387eefc5a 101 if (x.f > 4.0) {
kkulkarni36 0:7d7387eefc5a 102 s1 = -0.3;
kkulkarni36 0:7d7387eefc5a 103 s2 = 0.3;
kkulkarni36 0:7d7387eefc5a 104 m1.speed(s1);
kkulkarni36 0:7d7387eefc5a 105 m2.speed(s2);
kkulkarni36 0:7d7387eefc5a 106 myled1 = 0;
kkulkarni36 0:7d7387eefc5a 107 myled2 = 1;
kkulkarni36 0:7d7387eefc5a 108 myled3 = 0;
kkulkarni36 0:7d7387eefc5a 109 myled4 = 0;
kkulkarni36 0:7d7387eefc5a 110 wait(0.05);
kkulkarni36 0:7d7387eefc5a 111 }
kkulkarni36 0:7d7387eefc5a 112 if (y.f < -3.5) {
kkulkarni36 0:7d7387eefc5a 113 s1 = -0.2;
kkulkarni36 0:7d7387eefc5a 114 s2 = -0.2;
kkulkarni36 0:7d7387eefc5a 115 m1.speed(s1);
kkulkarni36 0:7d7387eefc5a 116 m2.speed(s2);
kkulkarni36 0:7d7387eefc5a 117 myled1 = 0;
kkulkarni36 0:7d7387eefc5a 118 myled2 = 0;
kkulkarni36 0:7d7387eefc5a 119 myled3 = 1;
kkulkarni36 0:7d7387eefc5a 120 myled4 = 0;
kkulkarni36 0:7d7387eefc5a 121 wait(0.05);
kkulkarni36 0:7d7387eefc5a 122 }
kkulkarni36 0:7d7387eefc5a 123 if (y.f > 3.0) {
kkulkarni36 0:7d7387eefc5a 124 s1 = 0.2;
kkulkarni36 0:7d7387eefc5a 125 s2 = 0.2;
kkulkarni36 0:7d7387eefc5a 126 m1.speed(s1);
kkulkarni36 0:7d7387eefc5a 127 m2.speed(s2);
kkulkarni36 0:7d7387eefc5a 128 myled1 = 0;
kkulkarni36 0:7d7387eefc5a 129 myled2 = 0;
kkulkarni36 0:7d7387eefc5a 130 myled3 = 0;
kkulkarni36 0:7d7387eefc5a 131 myled4 = 1;
kkulkarni36 0:7d7387eefc5a 132 wait(0.05);
kkulkarni36 0:7d7387eefc5a 133 }
kkulkarni36 0:7d7387eefc5a 134 if (x.f > -4.0 && x.f < 4.0 && y.f > -4.0 && y.f <4.0) {
kkulkarni36 0:7d7387eefc5a 135 s1 = 0.0;
kkulkarni36 0:7d7387eefc5a 136 s2 = 0.0;
kkulkarni36 0:7d7387eefc5a 137 m1.speed(s1);
kkulkarni36 0:7d7387eefc5a 138 m2.speed(s2);
kkulkarni36 0:7d7387eefc5a 139 wait(0.05);
kkulkarni36 0:7d7387eefc5a 140 }
kkulkarni36 0:7d7387eefc5a 141 while(move) {
kkulkarni36 0:7d7387eefc5a 142 sonar.checkDistance();
kkulkarni36 0:7d7387eefc5a 143 move = 0;
kkulkarni36 0:7d7387eefc5a 144 }
kkulkarni36 0:7d7387eefc5a 145 }
kkulkarni36 0:7d7387eefc5a 146 }
kkulkarni36 0:7d7387eefc5a 147 }
kkulkarni36 0:7d7387eefc5a 148 }
kkulkarni36 0:7d7387eefc5a 149 }