Robotics Interest Club
/
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
Revision 0:7a43aff338da, committed 2015-11-06
- Comitter:
- eljerchua
- Date:
- Fri Nov 06 12:47:28 2015 +0000
- Commit message:
- IR+BLUETOOTH
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sabertooth.lib Fri Nov 06 12:47:28 2015 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/Robotics-Interest-Club/code/Sabertooth/#f2a076d3ef77
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 06 12:47:28 2015 +0000
@@ -0,0 +1,124 @@
+#include "mbed.h"
+#include "Sabertooth.h"
+#include "peixing.h"
+
+Serial pc(USBTX, USBRX);
+Serial device(p28, p27);
+
+AnalogIn s1 (p15);
+AnalogIn s2 (p16);
+AnalogIn s3 (p19);
+AnalogIn s4 (p20);
+
+#define k_5 12466.0
+#define k_4 -23216.0
+#define k_3 14974.0
+#define k_2 -3585.0
+#define k_1 19.0
+#define k_0 96.0
+
+int i;
+float val1, dist1, total_dist1, ave_dist1;
+float val2, dist2, total_dist2, ave_dist2;
+
+void read_sensors()
+{
+ dist1 = 0;
+ total_dist1 = 0;
+ ave_dist1 = 0;
+
+ dist2 = 0;
+ total_dist2 = 0;
+ ave_dist2 = 0;
+
+ for(i = 0; i < 50; i++)
+ {
+ val1 = s1.read();
+ dist1 = 0;
+ dist1 += k_5*(val1*val1*val1*val1*val1);
+ dist1 += k_4*(val1*val1*val1*val1);
+ dist1 += k_3*(val1*val1*val1);
+ dist1 += k_2*(val1*val1);
+ dist1 += k_1*val1;
+ dist1 += k_0;
+ total_dist1 = total_dist1 + dist1;
+
+ val2 = s2.read();
+ dist2 = 0;
+ dist2 += k_5*(val2*val2*val2*val2*val2);
+ dist2 += k_4*(val2*val2*val2*val2);
+ dist2 += k_3*(val2*val2*val2);
+ dist2 += k_2*(val2*val2);
+ dist2 += k_1*val2;
+ dist2 += k_0;
+ total_dist2 = total_dist2 + dist2;
+ }
+ ave_dist1 = total_dist1 / 50;
+ ave_dist2 = total_dist2 / 50;
+}
+
+int main()
+{
+ pc.baud(115200);
+ device.baud(115200);
+ front_sb.InitializeCom();
+ back_sb.InitializeCom();
+ while(1)
+ {
+ read_sensors();
+ pc.putc(device.getc());
+ if(ave_dist1 >= 20 && ave_dist2 >= 20)
+ {
+ char c;
+ /*while(device.readable() == false)
+ {
+ read_sensors();
+ if(ave_dist1 < 20 || ave_dist2 < 20) break;
+ }
+ if(ave_dist1 >= 20 && ave_dist2 >= 20 && device.readable()) c = device.getc();*/
+ if(device.readable() == true) c = device.getc();
+ else c = 'p';
+ if(c == 'w') forward();
+ else if(c == 's') backward();
+ else if(c == 'd') right();
+ else if(c == 'a') left();
+ else if(c == 'l') turnright();
+ else if(c == 'k') turnleft();
+ else stop();
+ }
+ else if(ave_dist1 < 20 && ave_dist2 >= 20)
+ {
+ while(ave_dist1 < 20 && ave_dist2 >= 20)
+ {
+ read_sensors();
+ front_sb.SetSpeedMotorA(-30);
+ front_sb.SetSpeedMotorB(20);
+ back_sb.SetSpeedMotorA(20);
+ back_sb.SetSpeedMotorB(-30);
+ }
+ stop();
+ }
+ else if(ave_dist2 < 20 && ave_dist1 >= 20)
+ {
+ while(ave_dist2 < 20 && ave_dist1 >= 20)
+ {
+ read_sensors();
+ front_sb.SetSpeedMotorA(20);
+ front_sb.SetSpeedMotorB(-30);
+ back_sb.SetSpeedMotorA(-30);
+ back_sb.SetSpeedMotorB(20);
+ }
+ stop();
+ }
+ else if(ave_dist2 < 20 && ave_dist1 < 20)
+ {
+ while(ave_dist2 < 20 && ave_dist1 < 20)
+ {
+ read_sensors();
+ turnright();
+ }
+ stop();
+ }
+ else stop();
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 06 12:47:28 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/peixing.h Fri Nov 06 12:47:28 2015 +0000
@@ -0,0 +1,60 @@
+#include "Sabertooth.h"
+
+Sabertooth front_sb(p9, 128, 2400);
+Sabertooth back_sb(p13, 129, 9600);
+
+void forward()
+{
+ front_sb.SetSpeedMotorA(40);
+ front_sb.SetSpeedMotorB(50);
+ back_sb.SetSpeedMotorA(50);
+ back_sb.SetSpeedMotorB(50);
+}
+
+void backward()
+{
+ front_sb.SetSpeedMotorA(-40);
+ front_sb.SetSpeedMotorB(-50);
+ back_sb.SetSpeedMotorA(-50);
+ back_sb.SetSpeedMotorB(-50);
+}
+
+void right()
+{
+ front_sb.SetSpeedMotorA(-40);
+ front_sb.SetSpeedMotorB(50);
+ back_sb.SetSpeedMotorA(-50);
+ back_sb.SetSpeedMotorB(50);
+}
+
+void left()
+{
+ front_sb.SetSpeedMotorA(40);
+ front_sb.SetSpeedMotorB(-50);
+ back_sb.SetSpeedMotorA(50);
+ back_sb.SetSpeedMotorB(-50);
+}
+
+void turnright()
+{
+ front_sb.SetSpeedMotorA(-40);
+ front_sb.SetSpeedMotorB(50);
+ back_sb.SetSpeedMotorA(50);
+ back_sb.SetSpeedMotorB(-50);
+}
+
+void turnleft()
+{
+ front_sb.SetSpeedMotorA(40);
+ front_sb.SetSpeedMotorB(-50);
+ back_sb.SetSpeedMotorA(-50);
+ back_sb.SetSpeedMotorB(50);
+}
+
+void stop()
+{
+ front_sb.SetSpeedMotorA(0);
+ front_sb.SetSpeedMotorB(0);
+ back_sb.SetSpeedMotorA(0);
+ back_sb.SetSpeedMotorB(0);
+}
\ No newline at end of file