case การเดินของเต่า
Dependencies: Servo mbed-rtos mbed
main.cpp
- Committer:
- Khanchana
- Date:
- 2018-02-09
- Revision:
- 0:812929a5d5ad
File content as of revision 0:812929a5d5ad:
#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); } }