TurtleBot
case การเดินของเต่า
Dependencies: Servo mbed-rtos mbed
Revision 0:812929a5d5ad, committed 2018-02-09
- Comitter:
- Khanchana
- Date:
- Fri Feb 09 15:59:52 2018 +0000
- Commit message:
- ???????????? 5 ???
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Feb 09 15:59:52 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/jdenkers/code/Servo/#352133517ccc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Feb 09 15:59:52 2018 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+
+Serial pc(USBTX, USBRX);
+
+Servo Servo1(D7);
+Servo Servo2(D6);
+Servo Servo3(D9);
+Servo Servo4(D8);
+Thread thread;
+
+int pos_up_start;
+int pos_up_end;
+int pos_down_start;
+int pos_down_end;
+
+void myservoright_thread() {
+ for(int n = 0; n <= 5; n += 1){
+ Servo3.Enable(1000,20000);
+ Servo4.Disable();
+ for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
+ Servo3.SetPosition(pos);
+ wait(0.01);
+ }
+ Servo4.Enable(1000,20000);
+ Servo3.Disable();
+ for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
+ Servo4.SetPosition(pos);
+ wait(0.01);
+ }
+ Servo3.Enable(1000,20000);
+ Servo4.Disable();
+ for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
+ Servo3.SetPosition(pos);
+ wait(0.01);
+ }
+ Servo4.Enable(1000,20000);
+ Servo3.Disable();
+ for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
+ Servo4.SetPosition(pos);
+ wait(0.01);
+ }
+ }
+}
+
+void turtlewalk(int pos_down_start,int pos_down_end,int pos_up_start,int pos_up_end){
+ for(int n = 0; n <= 5; n += 1){
+ Servo1.Enable(1000,20000);
+ Servo2.Disable();
+ thread.start(myservoright_thread);
+ for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
+ Servo1.SetPosition(pos);
+ wait(0.01);
+ }
+ Servo2.Enable(1000,20000);
+ Servo1.Disable();
+ for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
+ Servo2.SetPosition(pos);
+ wait(0.01);
+ }
+ Servo1.Enable(1000,20000);
+ Servo2.Disable();
+ for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
+ Servo1.SetPosition(pos);
+ wait(0.01);
+ }
+ Servo2.Enable(1000,20000);
+ Servo1.Disable();
+ for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
+ Servo2.SetPosition(pos);
+ wait(0.01);
+ }
+ }
+}
+
+int main(){
+
+ while(1) {
+ printf("Hello World! Turtlebot is READY\n");
+ printf("case 1-5\n");
+ switch(pc.getc()) {
+ case '1':
+ pos_down_start = 1400;
+ pos_down_end = 1700;
+ pos_up_start = 1000;
+ pos_up_end = 1700;
+ break;
+ case '2':
+ pos_down_start = 1400;
+ pos_down_end = 1600;
+ pos_up_start = 1000;
+ pos_up_end = 1600;
+ break;
+ case '3':
+ pos_down_start = 1400;
+ pos_down_end = 1650;
+ pos_up_start = 1000;
+ pos_up_end = 1500;
+ break;
+ case '4':
+ pos_down_start = 1400;
+ pos_down_end = 1700;
+ pos_up_start = 1000;
+ pos_up_end = 1650;
+ break;
+ case '5':
+ pos_down_start = 1400;
+ pos_down_end = 1600;
+ pos_up_start = 1000;
+ pos_up_end = 1550;
+ break;
+ }
+ printf("position down motor start = %d\n", pos_down_start);
+ printf("position down motor end = %d\n", pos_down_end);
+ printf("position up motor start = %d\n", pos_up_start);
+ printf("position up motor end = %d\n", pos_up_end);
+ turtlewalk(pos_down_start,pos_down_end,pos_up_start,pos_up_end);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Fri Feb 09 15:59:52 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Feb 09 15:59:52 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/7130f322cb7e \ No newline at end of file