Robotics Interest Club / Mbed 2 deprecated Open_House_IR_Bluetooth

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:   Sabertooth mbed

main.cpp@0:7a43aff338da, 2015-11-06 (annotated)

Committer:
eljerchua
Date:
Fri Nov 06 12:47:28 2015 +0000
Revision:
0:7a43aff338da
IR+BLUETOOTH

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eljerchua 0:7a43aff338da 1 #include "mbed.h"
eljerchua 0:7a43aff338da 2 #include "Sabertooth.h"
eljerchua 0:7a43aff338da 3 #include "peixing.h"
eljerchua 0:7a43aff338da 4
eljerchua 0:7a43aff338da 5 Serial pc(USBTX, USBRX);
eljerchua 0:7a43aff338da 6 Serial device(p28, p27);
eljerchua 0:7a43aff338da 7
eljerchua 0:7a43aff338da 8 AnalogIn s1 (p15);
eljerchua 0:7a43aff338da 9 AnalogIn s2 (p16);
eljerchua 0:7a43aff338da 10 AnalogIn s3 (p19);
eljerchua 0:7a43aff338da 11 AnalogIn s4 (p20);
eljerchua 0:7a43aff338da 12
eljerchua 0:7a43aff338da 13 #define k_5 12466.0
eljerchua 0:7a43aff338da 14 #define k_4 -23216.0
eljerchua 0:7a43aff338da 15 #define k_3 14974.0
eljerchua 0:7a43aff338da 16 #define k_2 -3585.0
eljerchua 0:7a43aff338da 17 #define k_1 19.0
eljerchua 0:7a43aff338da 18 #define k_0 96.0
eljerchua 0:7a43aff338da 19
eljerchua 0:7a43aff338da 20 int i;
eljerchua 0:7a43aff338da 21 float val1, dist1, total_dist1, ave_dist1;
eljerchua 0:7a43aff338da 22 float val2, dist2, total_dist2, ave_dist2;
eljerchua 0:7a43aff338da 23
eljerchua 0:7a43aff338da 24 void read_sensors()
eljerchua 0:7a43aff338da 25 {
eljerchua 0:7a43aff338da 26 dist1 = 0;
eljerchua 0:7a43aff338da 27 total_dist1 = 0;
eljerchua 0:7a43aff338da 28 ave_dist1 = 0;
eljerchua 0:7a43aff338da 29
eljerchua 0:7a43aff338da 30 dist2 = 0;
eljerchua 0:7a43aff338da 31 total_dist2 = 0;
eljerchua 0:7a43aff338da 32 ave_dist2 = 0;
eljerchua 0:7a43aff338da 33
eljerchua 0:7a43aff338da 34 for(i = 0; i < 50; i++)
eljerchua 0:7a43aff338da 35 {
eljerchua 0:7a43aff338da 36 val1 = s1.read();
eljerchua 0:7a43aff338da 37 dist1 = 0;
eljerchua 0:7a43aff338da 38 dist1 += k_5*(val1*val1*val1*val1*val1);
eljerchua 0:7a43aff338da 39 dist1 += k_4*(val1*val1*val1*val1);
eljerchua 0:7a43aff338da 40 dist1 += k_3*(val1*val1*val1);
eljerchua 0:7a43aff338da 41 dist1 += k_2*(val1*val1);
eljerchua 0:7a43aff338da 42 dist1 += k_1*val1;
eljerchua 0:7a43aff338da 43 dist1 += k_0;
eljerchua 0:7a43aff338da 44 total_dist1 = total_dist1 + dist1;
eljerchua 0:7a43aff338da 45
eljerchua 0:7a43aff338da 46 val2 = s2.read();
eljerchua 0:7a43aff338da 47 dist2 = 0;
eljerchua 0:7a43aff338da 48 dist2 += k_5*(val2*val2*val2*val2*val2);
eljerchua 0:7a43aff338da 49 dist2 += k_4*(val2*val2*val2*val2);
eljerchua 0:7a43aff338da 50 dist2 += k_3*(val2*val2*val2);
eljerchua 0:7a43aff338da 51 dist2 += k_2*(val2*val2);
eljerchua 0:7a43aff338da 52 dist2 += k_1*val2;
eljerchua 0:7a43aff338da 53 dist2 += k_0;
eljerchua 0:7a43aff338da 54 total_dist2 = total_dist2 + dist2;
eljerchua 0:7a43aff338da 55 }
eljerchua 0:7a43aff338da 56 ave_dist1 = total_dist1 / 50;
eljerchua 0:7a43aff338da 57 ave_dist2 = total_dist2 / 50;
eljerchua 0:7a43aff338da 58 }
eljerchua 0:7a43aff338da 59
eljerchua 0:7a43aff338da 60 int main()
eljerchua 0:7a43aff338da 61 {
eljerchua 0:7a43aff338da 62 pc.baud(115200);
eljerchua 0:7a43aff338da 63 device.baud(115200);
eljerchua 0:7a43aff338da 64 front_sb.InitializeCom();
eljerchua 0:7a43aff338da 65 back_sb.InitializeCom();
eljerchua 0:7a43aff338da 66 while(1)
eljerchua 0:7a43aff338da 67 {
eljerchua 0:7a43aff338da 68 read_sensors();
eljerchua 0:7a43aff338da 69 pc.putc(device.getc());
eljerchua 0:7a43aff338da 70 if(ave_dist1 >= 20 && ave_dist2 >= 20)
eljerchua 0:7a43aff338da 71 {
eljerchua 0:7a43aff338da 72 char c;
eljerchua 0:7a43aff338da 73 /*while(device.readable() == false)
eljerchua 0:7a43aff338da 74 {
eljerchua 0:7a43aff338da 75 read_sensors();
eljerchua 0:7a43aff338da 76 if(ave_dist1 < 20 || ave_dist2 < 20) break;
eljerchua 0:7a43aff338da 77 }
eljerchua 0:7a43aff338da 78 if(ave_dist1 >= 20 && ave_dist2 >= 20 && device.readable()) c = device.getc();*/
eljerchua 0:7a43aff338da 79 if(device.readable() == true) c = device.getc();
eljerchua 0:7a43aff338da 80 else c = 'p';
eljerchua 0:7a43aff338da 81 if(c == 'w') forward();
eljerchua 0:7a43aff338da 82 else if(c == 's') backward();
eljerchua 0:7a43aff338da 83 else if(c == 'd') right();
eljerchua 0:7a43aff338da 84 else if(c == 'a') left();
eljerchua 0:7a43aff338da 85 else if(c == 'l') turnright();
eljerchua 0:7a43aff338da 86 else if(c == 'k') turnleft();
eljerchua 0:7a43aff338da 87 else stop();
eljerchua 0:7a43aff338da 88 }
eljerchua 0:7a43aff338da 89 else if(ave_dist1 < 20 && ave_dist2 >= 20)
eljerchua 0:7a43aff338da 90 {
eljerchua 0:7a43aff338da 91 while(ave_dist1 < 20 && ave_dist2 >= 20)
eljerchua 0:7a43aff338da 92 {
eljerchua 0:7a43aff338da 93 read_sensors();
eljerchua 0:7a43aff338da 94 front_sb.SetSpeedMotorA(-30);
eljerchua 0:7a43aff338da 95 front_sb.SetSpeedMotorB(20);
eljerchua 0:7a43aff338da 96 back_sb.SetSpeedMotorA(20);
eljerchua 0:7a43aff338da 97 back_sb.SetSpeedMotorB(-30);
eljerchua 0:7a43aff338da 98 }
eljerchua 0:7a43aff338da 99 stop();
eljerchua 0:7a43aff338da 100 }
eljerchua 0:7a43aff338da 101 else if(ave_dist2 < 20 && ave_dist1 >= 20)
eljerchua 0:7a43aff338da 102 {
eljerchua 0:7a43aff338da 103 while(ave_dist2 < 20 && ave_dist1 >= 20)
eljerchua 0:7a43aff338da 104 {
eljerchua 0:7a43aff338da 105 read_sensors();
eljerchua 0:7a43aff338da 106 front_sb.SetSpeedMotorA(20);
eljerchua 0:7a43aff338da 107 front_sb.SetSpeedMotorB(-30);
eljerchua 0:7a43aff338da 108 back_sb.SetSpeedMotorA(-30);
eljerchua 0:7a43aff338da 109 back_sb.SetSpeedMotorB(20);
eljerchua 0:7a43aff338da 110 }
eljerchua 0:7a43aff338da 111 stop();
eljerchua 0:7a43aff338da 112 }
eljerchua 0:7a43aff338da 113 else if(ave_dist2 < 20 && ave_dist1 < 20)
eljerchua 0:7a43aff338da 114 {
eljerchua 0:7a43aff338da 115 while(ave_dist2 < 20 && ave_dist1 < 20)
eljerchua 0:7a43aff338da 116 {
eljerchua 0:7a43aff338da 117 read_sensors();
eljerchua 0:7a43aff338da 118 turnright();
eljerchua 0:7a43aff338da 119 }
eljerchua 0:7a43aff338da 120 stop();
eljerchua 0:7a43aff338da 121 }
eljerchua 0:7a43aff338da 122 else stop();
eljerchua 0:7a43aff338da 123 }
eljerchua 0:7a43aff338da 124 }