eljer chua
aaa
Dependencies: LidarLitev2 Sabertoothaaa mbed-rtos mbed
Revision 0:59ae1ba97a94, committed 2017-09-28
- Comitter:
- eljerchua
- Date:
- Thu Sep 28 02:07:41 2017 +0000
- Commit message:
- aaa
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LidarLitev2.lib Thu Sep 28 02:07:41 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/sventura3/code/LidarLitev2/#7e6c07be1754
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sabertooth.lib Thu Sep 28 02:07:41 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/eljerchua/code/Sabertoothaaa/#c22f68fd1b55
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Sep 28 02:07:41 2017 +0000
@@ -0,0 +1,144 @@
+#include "mbed.h"
+#include "Sabertooth.h"
+#include "rtos.h"
+#include "peixingv2.h"
+#include "LidarLitev2.h"
+
+Serial pc(USBTX, USBRX);
+Serial device(p28, p27);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+DigitalIn SignalIn(p29);
+DigitalOut SignalOut(p30);
+LidarLitev2 Lidar(p9, p10);
+PwmOut servo(p21);
+
+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;
+bool x;
+
+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;
+}
+
+void thread1(void const *args)
+{
+ while(true)
+ {
+ read_sensors();
+ int speed = 20;
+ if(ave_dist1 < 30 || ave_dist2 < 30) SignalOut = 1;
+ else SignalOut = 0;
+ if(ave_dist1 < 30 && ave_dist2 >= 30)
+ {
+ x = false;
+ turnright(speed);
+ }
+ else if(ave_dist2 < 30 && ave_dist1 >= 30)
+ {
+ x = false;
+ turnleft(speed);
+ }
+ else if(ave_dist2 < 30 && ave_dist1 < 30)
+ {
+ x = false;
+ turnright(speed);
+ }
+ else x = true;
+ }
+}
+
+int main()
+{
+ pc.baud(115200);
+ device.baud(115200);
+ front_sb.InitializeCom();
+ back_sb.InitializeCom();
+ Thread thread(thread1);
+ int speed = 50;
+ float distance;
+ char c = 'k', a;
+ Lidar.configure();
+ servo.period_ms(20);
+ while(true)
+ {
+ if(x == true)
+ {
+ if(device.readable())
+ {
+ a = device.getc();
+ if(a > 0 && a <= 100) speed = a;
+ else if(a == 108)
+ {
+ distance = 0;
+ for(int j = 0; j < 3; j++) distance = distance + Lidar.distance();
+ distance = distance / 3;
+ pc.printf("%d", int(distance));
+ device.putc(int(distance));
+ }
+ else if(a >= 109)
+ {
+ servo.pulsewidth_us(1089 + (a - 109) * 7);
+ wait(0.01);
+ }
+ else c = a;
+ }
+ if(c == 'g') forward(speed);
+ else if(c == 'h') backward(speed);
+ else if(c == 'j') right(speed);
+ else if(c == 'i') left(speed);
+ else if(c == 'e') turnright(speed);
+ else if(c == 'f') turnleft(speed);
+ else stop();
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Sep 28 02:07:41 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed-rtos/#12552ef4e980
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Sep 28 02:07:41 2017 +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/peixingv2.h Thu Sep 28 02:07:41 2017 +0000
@@ -0,0 +1,60 @@
+#include "Sabertooth.h"
+
+Sabertooth front_sb(p13, 128, 9600);
+Sabertooth back_sb(p13, 129, 9600);
+
+void forward(int speed)
+{
+ front_sb.SetSpeedMotorA(speed);
+ front_sb.SetSpeedMotorB(speed);
+ back_sb.SetSpeedMotorA(speed);
+ back_sb.SetSpeedMotorB(speed);
+}
+
+void backward(int speed)
+{
+ front_sb.SetSpeedMotorA(-speed);
+ front_sb.SetSpeedMotorB(-speed);
+ back_sb.SetSpeedMotorA(-speed);
+ back_sb.SetSpeedMotorB(-speed);
+}
+
+void right(int speed)
+{
+ front_sb.SetSpeedMotorA(-speed);
+ front_sb.SetSpeedMotorB(speed);
+ back_sb.SetSpeedMotorA(-speed);
+ back_sb.SetSpeedMotorB(speed);
+}
+
+void left(int speed)
+{
+ front_sb.SetSpeedMotorA(speed);
+ front_sb.SetSpeedMotorB(-speed);
+ back_sb.SetSpeedMotorA(speed);
+ back_sb.SetSpeedMotorB(-speed);
+}
+
+void turnright(int speed)
+{
+ front_sb.SetSpeedMotorA(-speed);
+ front_sb.SetSpeedMotorB(speed);
+ back_sb.SetSpeedMotorA(speed);
+ back_sb.SetSpeedMotorB(-speed);
+}
+
+void turnleft(int speed)
+{
+ front_sb.SetSpeedMotorA(speed);
+ front_sb.SetSpeedMotorB(-speed);
+ back_sb.SetSpeedMotorA(-speed);
+ back_sb.SetSpeedMotorB(speed);
+}
+
+void stop()
+{
+ front_sb.SetSpeedMotorA(0);
+ front_sb.SetSpeedMotorB(0);
+ back_sb.SetSpeedMotorA(0);
+ back_sb.SetSpeedMotorB(0);
+}
\ No newline at end of file