TurtleBot / Mbed 2 deprecated Turtlecase

case การเดินของเต่า

Dependencies:   Servo mbed-rtos mbed

main.cpp@0:812929a5d5ad, 2018-02-09 (annotated)

Committer:
Khanchana
Date:
Fri Feb 09 15:59:52 2018 +0000
Revision:
0:812929a5d5ad
???????????? 5 ???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khanchana 0:812929a5d5ad 1 #include "mbed.h"
Khanchana 0:812929a5d5ad 2 #include "Servo.h"
Khanchana 0:812929a5d5ad 3 #include "rtos.h"
Khanchana 0:812929a5d5ad 4
Khanchana 0:812929a5d5ad 5 Serial pc(USBTX, USBRX);
Khanchana 0:812929a5d5ad 6
Khanchana 0:812929a5d5ad 7 Servo Servo1(D7);
Khanchana 0:812929a5d5ad 8 Servo Servo2(D6);
Khanchana 0:812929a5d5ad 9 Servo Servo3(D9);
Khanchana 0:812929a5d5ad 10 Servo Servo4(D8);
Khanchana 0:812929a5d5ad 11 Thread thread;
Khanchana 0:812929a5d5ad 12
Khanchana 0:812929a5d5ad 13 int pos_up_start;
Khanchana 0:812929a5d5ad 14 int pos_up_end;
Khanchana 0:812929a5d5ad 15 int pos_down_start;
Khanchana 0:812929a5d5ad 16 int pos_down_end;
Khanchana 0:812929a5d5ad 17
Khanchana 0:812929a5d5ad 18 void myservoright_thread() {
Khanchana 0:812929a5d5ad 19 for(int n = 0; n <= 5; n += 1){
Khanchana 0:812929a5d5ad 20 Servo3.Enable(1000,20000);
Khanchana 0:812929a5d5ad 21 Servo4.Disable();
Khanchana 0:812929a5d5ad 22 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
Khanchana 0:812929a5d5ad 23 Servo3.SetPosition(pos);
Khanchana 0:812929a5d5ad 24 wait(0.01);
Khanchana 0:812929a5d5ad 25 }
Khanchana 0:812929a5d5ad 26 Servo4.Enable(1000,20000);
Khanchana 0:812929a5d5ad 27 Servo3.Disable();
Khanchana 0:812929a5d5ad 28 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
Khanchana 0:812929a5d5ad 29 Servo4.SetPosition(pos);
Khanchana 0:812929a5d5ad 30 wait(0.01);
Khanchana 0:812929a5d5ad 31 }
Khanchana 0:812929a5d5ad 32 Servo3.Enable(1000,20000);
Khanchana 0:812929a5d5ad 33 Servo4.Disable();
Khanchana 0:812929a5d5ad 34 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
Khanchana 0:812929a5d5ad 35 Servo3.SetPosition(pos);
Khanchana 0:812929a5d5ad 36 wait(0.01);
Khanchana 0:812929a5d5ad 37 }
Khanchana 0:812929a5d5ad 38 Servo4.Enable(1000,20000);
Khanchana 0:812929a5d5ad 39 Servo3.Disable();
Khanchana 0:812929a5d5ad 40 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
Khanchana 0:812929a5d5ad 41 Servo4.SetPosition(pos);
Khanchana 0:812929a5d5ad 42 wait(0.01);
Khanchana 0:812929a5d5ad 43 }
Khanchana 0:812929a5d5ad 44 }
Khanchana 0:812929a5d5ad 45 }
Khanchana 0:812929a5d5ad 46
Khanchana 0:812929a5d5ad 47 void turtlewalk(int pos_down_start,int pos_down_end,int pos_up_start,int pos_up_end){
Khanchana 0:812929a5d5ad 48 for(int n = 0; n <= 5; n += 1){
Khanchana 0:812929a5d5ad 49 Servo1.Enable(1000,20000);
Khanchana 0:812929a5d5ad 50 Servo2.Disable();
Khanchana 0:812929a5d5ad 51 thread.start(myservoright_thread);
Khanchana 0:812929a5d5ad 52 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
Khanchana 0:812929a5d5ad 53 Servo1.SetPosition(pos);
Khanchana 0:812929a5d5ad 54 wait(0.01);
Khanchana 0:812929a5d5ad 55 }
Khanchana 0:812929a5d5ad 56 Servo2.Enable(1000,20000);
Khanchana 0:812929a5d5ad 57 Servo1.Disable();
Khanchana 0:812929a5d5ad 58 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
Khanchana 0:812929a5d5ad 59 Servo2.SetPosition(pos);
Khanchana 0:812929a5d5ad 60 wait(0.01);
Khanchana 0:812929a5d5ad 61 }
Khanchana 0:812929a5d5ad 62 Servo1.Enable(1000,20000);
Khanchana 0:812929a5d5ad 63 Servo2.Disable();
Khanchana 0:812929a5d5ad 64 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
Khanchana 0:812929a5d5ad 65 Servo1.SetPosition(pos);
Khanchana 0:812929a5d5ad 66 wait(0.01);
Khanchana 0:812929a5d5ad 67 }
Khanchana 0:812929a5d5ad 68 Servo2.Enable(1000,20000);
Khanchana 0:812929a5d5ad 69 Servo1.Disable();
Khanchana 0:812929a5d5ad 70 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
Khanchana 0:812929a5d5ad 71 Servo2.SetPosition(pos);
Khanchana 0:812929a5d5ad 72 wait(0.01);
Khanchana 0:812929a5d5ad 73 }
Khanchana 0:812929a5d5ad 74 }
Khanchana 0:812929a5d5ad 75 }
Khanchana 0:812929a5d5ad 76
Khanchana 0:812929a5d5ad 77 int main(){
Khanchana 0:812929a5d5ad 78
Khanchana 0:812929a5d5ad 79 while(1) {
Khanchana 0:812929a5d5ad 80 printf("Hello World! Turtlebot is READY\n");
Khanchana 0:812929a5d5ad 81 printf("case 1-5\n");
Khanchana 0:812929a5d5ad 82 switch(pc.getc()) {
Khanchana 0:812929a5d5ad 83 case '1':
Khanchana 0:812929a5d5ad 84 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 85 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 86 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 87 pos_up_end = 1700;
Khanchana 0:812929a5d5ad 88 break;
Khanchana 0:812929a5d5ad 89 case '2':
Khanchana 0:812929a5d5ad 90 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 91 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 92 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 93 pos_up_end = 1600;
Khanchana 0:812929a5d5ad 94 break;
Khanchana 0:812929a5d5ad 95 case '3':
Khanchana 0:812929a5d5ad 96 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 97 pos_down_end = 1650;
Khanchana 0:812929a5d5ad 98 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 99 pos_up_end = 1500;
Khanchana 0:812929a5d5ad 100 break;
Khanchana 0:812929a5d5ad 101 case '4':
Khanchana 0:812929a5d5ad 102 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 103 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 104 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 105 pos_up_end = 1650;
Khanchana 0:812929a5d5ad 106 break;
Khanchana 0:812929a5d5ad 107 case '5':
Khanchana 0:812929a5d5ad 108 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 109 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 110 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 111 pos_up_end = 1550;
Khanchana 0:812929a5d5ad 112 break;
Khanchana 0:812929a5d5ad 113 }
Khanchana 0:812929a5d5ad 114 printf("position down motor start = %d\n", pos_down_start);
Khanchana 0:812929a5d5ad 115 printf("position down motor end = %d\n", pos_down_end);
Khanchana 0:812929a5d5ad 116 printf("position up motor start = %d\n", pos_up_start);
Khanchana 0:812929a5d5ad 117 printf("position up motor end = %d\n", pos_up_end);
Khanchana 0:812929a5d5ad 118 turtlewalk(pos_down_start,pos_down_end,pos_up_start,pos_up_end);
Khanchana 0:812929a5d5ad 119 }
Khanchana 0:812929a5d5ad 120 }