Will Trimmer / Mbed 2 deprecated ece4180-project

MBED robot with collision detection

Dependencies:   mbed mbed-rtos Motor HC_SR04_Ultrasonic_Library

main.cpp@0:1676cb5a5d33, 2018-12-12 (annotated)

Committer:
wtrimmer3
Date:
Wed Dec 12 00:10:20 2018 +0000
Revision:
0:1676cb5a5d33
project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wtrimmer3 0:1676cb5a5d33 1 #include "mbed.h"
wtrimmer3 0:1676cb5a5d33 2 #include "Motor.h"
wtrimmer3 0:1676cb5a5d33 3 #include "rtos.h"
wtrimmer3 0:1676cb5a5d33 4 #include "ultrasonic.h"
wtrimmer3 0:1676cb5a5d33 5
wtrimmer3 0:1676cb5a5d33 6 Serial bluemod(p13,p14); //change blt to this
wtrimmer3 0:1676cb5a5d33 7 PwmOut left_led(LED1);
wtrimmer3 0:1676cb5a5d33 8 PwmOut right_led(LED4);
wtrimmer3 0:1676cb5a5d33 9 PwmOut forward_led(LED2);
wtrimmer3 0:1676cb5a5d33 10 PwmOut reverse_led(LED3);
wtrimmer3 0:1676cb5a5d33 11
wtrimmer3 0:1676cb5a5d33 12 DigitalOut trig(p6);
wtrimmer3 0:1676cb5a5d33 13 DigitalOut echo(p7);
wtrimmer3 0:1676cb5a5d33 14
wtrimmer3 0:1676cb5a5d33 15 Serial pc(USBTX, USBRX);
wtrimmer3 0:1676cb5a5d33 16
wtrimmer3 0:1676cb5a5d33 17 Motor m1(p26, p25, p24); // left motor : pwm, fwd, rev
wtrimmer3 0:1676cb5a5d33 18 Motor m2(p21, p22, p23); // right motor : pwm, fwd, rev
wtrimmer3 0:1676cb5a5d33 19
wtrimmer3 0:1676cb5a5d33 20 bool lidarFlag1 = 0;
wtrimmer3 0:1676cb5a5d33 21 int flag_distance;
wtrimmer3 0:1676cb5a5d33 22 int g_distance;
wtrimmer3 0:1676cb5a5d33 23 void dist(int distance) {
wtrimmer3 0:1676cb5a5d33 24 g_distance = distance;
wtrimmer3 0:1676cb5a5d33 25 if(g_distance < flag_distance) lidarFlag1 = 1;
wtrimmer3 0:1676cb5a5d33 26 pc.printf("Distance %d mm\r\n", g_distance);
wtrimmer3 0:1676cb5a5d33 27 }
wtrimmer3 0:1676cb5a5d33 28 ultrasonic mu(p6, p7, .1, 1, &dist);
wtrimmer3 0:1676cb5a5d33 29
wtrimmer3 0:1676cb5a5d33 30 //C union can convert 4 chars to a float - puts them in same location in memory
wtrimmer3 0:1676cb5a5d33 31 //trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float
wtrimmer3 0:1676cb5a5d33 32 union f_or_char {
wtrimmer3 0:1676cb5a5d33 33 float f;
wtrimmer3 0:1676cb5a5d33 34 char c[4];
wtrimmer3 0:1676cb5a5d33 35 };
wtrimmer3 0:1676cb5a5d33 36
wtrimmer3 0:1676cb5a5d33 37 void calcSpeed(float x, float y, float th) {
wtrimmer3 0:1676cb5a5d33 38 float left;
wtrimmer3 0:1676cb5a5d33 39 float right;
wtrimmer3 0:1676cb5a5d33 40 if (x > th) { //moving forward
wtrimmer3 0:1676cb5a5d33 41 left = x;
wtrimmer3 0:1676cb5a5d33 42 right = x;
wtrimmer3 0:1676cb5a5d33 43 } else if (x < -th) { //reverse
wtrimmer3 0:1676cb5a5d33 44 left = x;
wtrimmer3 0:1676cb5a5d33 45 right = x;
wtrimmer3 0:1676cb5a5d33 46 } else {
wtrimmer3 0:1676cb5a5d33 47 left = 0.0;
wtrimmer3 0:1676cb5a5d33 48 right = 0.0;
wtrimmer3 0:1676cb5a5d33 49 }
wtrimmer3 0:1676cb5a5d33 50
wtrimmer3 0:1676cb5a5d33 51 flag_distance = left * 400;
wtrimmer3 0:1676cb5a5d33 52 if (flag_distance < 250) flag_distance = 250;
wtrimmer3 0:1676cb5a5d33 53
wtrimmer3 0:1676cb5a5d33 54 //turning
wtrimmer3 0:1676cb5a5d33 55 if (y > th) {//turning left
wtrimmer3 0:1676cb5a5d33 56 if (x > 0.2) {
wtrimmer3 0:1676cb5a5d33 57 right += y/2;
wtrimmer3 0:1676cb5a5d33 58 left -= y/2;
wtrimmer3 0:1676cb5a5d33 59 } else {
wtrimmer3 0:1676cb5a5d33 60 right -= y/2;
wtrimmer3 0:1676cb5a5d33 61 left += y/2;
wtrimmer3 0:1676cb5a5d33 62 }
wtrimmer3 0:1676cb5a5d33 63 } else if (y < -th) { //turning right
wtrimmer3 0:1676cb5a5d33 64 if (x > 0.2) {
wtrimmer3 0:1676cb5a5d33 65 left -= y/2;
wtrimmer3 0:1676cb5a5d33 66 right += y/2;
wtrimmer3 0:1676cb5a5d33 67 } else {
wtrimmer3 0:1676cb5a5d33 68 left += y/2;
wtrimmer3 0:1676cb5a5d33 69 right -= y/2;
wtrimmer3 0:1676cb5a5d33 70 }
wtrimmer3 0:1676cb5a5d33 71 }
wtrimmer3 0:1676cb5a5d33 72
wtrimmer3 0:1676cb5a5d33 73 //check that speed is within the range [-1,1]
wtrimmer3 0:1676cb5a5d33 74 if (left > 1.0) {
wtrimmer3 0:1676cb5a5d33 75 left = 1.0;
wtrimmer3 0:1676cb5a5d33 76 } else if (left < -1.0) {
wtrimmer3 0:1676cb5a5d33 77 left = -1.0;
wtrimmer3 0:1676cb5a5d33 78 }
wtrimmer3 0:1676cb5a5d33 79 //check that speed is within the range [-1,1]
wtrimmer3 0:1676cb5a5d33 80 if (right > 1.0) {
wtrimmer3 0:1676cb5a5d33 81 right = 1.0;
wtrimmer3 0:1676cb5a5d33 82 } else if (right < -1.0) {
wtrimmer3 0:1676cb5a5d33 83 right = -1.0;
wtrimmer3 0:1676cb5a5d33 84 }
wtrimmer3 0:1676cb5a5d33 85
wtrimmer3 0:1676cb5a5d33 86 //set speed
wtrimmer3 0:1676cb5a5d33 87 m1.speed(left);
wtrimmer3 0:1676cb5a5d33 88 m2.speed(right);
wtrimmer3 0:1676cb5a5d33 89 }
wtrimmer3 0:1676cb5a5d33 90
wtrimmer3 0:1676cb5a5d33 91 void speed()
wtrimmer3 0:1676cb5a5d33 92 {
wtrimmer3 0:1676cb5a5d33 93 char bchecksum=0;
wtrimmer3 0:1676cb5a5d33 94 char temp=0;
wtrimmer3 0:1676cb5a5d33 95
wtrimmer3 0:1676cb5a5d33 96 union f_or_char x,y,z;
wtrimmer3 0:1676cb5a5d33 97 while(1) {
wtrimmer3 0:1676cb5a5d33 98 bchecksum=0;
wtrimmer3 0:1676cb5a5d33 99 if (bluemod.getc()=='!') {
wtrimmer3 0:1676cb5a5d33 100 if (bluemod.getc()=='A') { //Accelerometer data packet
wtrimmer3 0:1676cb5a5d33 101 for (int i=0; i<4; i++) {
wtrimmer3 0:1676cb5a5d33 102 temp = bluemod.getc();
wtrimmer3 0:1676cb5a5d33 103 x.c[i] = temp;
wtrimmer3 0:1676cb5a5d33 104 bchecksum = bchecksum + temp;
wtrimmer3 0:1676cb5a5d33 105 }
wtrimmer3 0:1676cb5a5d33 106 for (int i=0; i<4; i++) {
wtrimmer3 0:1676cb5a5d33 107 temp = bluemod.getc();
wtrimmer3 0:1676cb5a5d33 108 y.c[i] = temp;
wtrimmer3 0:1676cb5a5d33 109 bchecksum = bchecksum + temp;
wtrimmer3 0:1676cb5a5d33 110 }
wtrimmer3 0:1676cb5a5d33 111 for (int i=0; i<4; i++) {
wtrimmer3 0:1676cb5a5d33 112 temp = bluemod.getc();
wtrimmer3 0:1676cb5a5d33 113 z.c[i] = temp;
wtrimmer3 0:1676cb5a5d33 114 bchecksum = bchecksum + temp;
wtrimmer3 0:1676cb5a5d33 115 }
wtrimmer3 0:1676cb5a5d33 116 if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK?
wtrimmer3 0:1676cb5a5d33 117 //printf("X = %f Y = %f Z = %f\n\r",x.f, y.f, z.f);
wtrimmer3 0:1676cb5a5d33 118 right_led = left_led = forward_led = reverse_led = 0.0;
wtrimmer3 0:1676cb5a5d33 119 if (x.f<=0.0) right_led = -x.f/10.0;//Scale to 0.0 to 1.0 for PWM so /10.0
wtrimmer3 0:1676cb5a5d33 120 else left_led = x.f/10.0;
wtrimmer3 0:1676cb5a5d33 121 if (y.f<=0.0) forward_led = -y.f/10.0;
wtrimmer3 0:1676cb5a5d33 122 else reverse_led = y.f/10.0;
wtrimmer3 0:1676cb5a5d33 123
wtrimmer3 0:1676cb5a5d33 124 if(!lidarFlag1) calcSpeed(x.f, y.f, 0.2f);
wtrimmer3 0:1676cb5a5d33 125 }
wtrimmer3 0:1676cb5a5d33 126 }
wtrimmer3 0:1676cb5a5d33 127 }
wtrimmer3 0:1676cb5a5d33 128 }
wtrimmer3 0:1676cb5a5d33 129 }
wtrimmer3 0:1676cb5a5d33 130
wtrimmer3 0:1676cb5a5d33 131 int main()
wtrimmer3 0:1676cb5a5d33 132 {
wtrimmer3 0:1676cb5a5d33 133 Thread thread1;
wtrimmer3 0:1676cb5a5d33 134 thread1.start(speed);
wtrimmer3 0:1676cb5a5d33 135 mu.startUpdates();
wtrimmer3 0:1676cb5a5d33 136 while(1) {
wtrimmer3 0:1676cb5a5d33 137 mu.checkDistance();
wtrimmer3 0:1676cb5a5d33 138 if(lidarFlag1==1) {
wtrimmer3 0:1676cb5a5d33 139 while(g_distance < 250) {
wtrimmer3 0:1676cb5a5d33 140 mu.checkDistance();
wtrimmer3 0:1676cb5a5d33 141 m1.speed(-0.2);
wtrimmer3 0:1676cb5a5d33 142 m2.speed(-0.2);
wtrimmer3 0:1676cb5a5d33 143 }
wtrimmer3 0:1676cb5a5d33 144 m1.speed(0.0);
wtrimmer3 0:1676cb5a5d33 145 m2.speed(0.0);
wtrimmer3 0:1676cb5a5d33 146 lidarFlag1 = 0;
wtrimmer3 0:1676cb5a5d33 147 }
wtrimmer3 0:1676cb5a5d33 148 }
wtrimmer3 0:1676cb5a5d33 149 }