Diff: main.cpp

Revision:

0:84ddd6d354e1

Child:

1:290e621741fd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Dec 22 09:27:29 2018 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+
+#define servo_NEUTRAL_R    1460
+#define servo_NEUTRAL_L    1460
+#define servo_FORWARD_R    1860
+#define servo_FORWARD_L    1860
+#define servo_back_R       1060
+#define servo_back_L       1060
+#define servo_slow_FORWARD_R    1560
+#define servo_slow_FORWARD_L    1560
+#define servo_slow_back_R       1360
+#define servo_slow_back_L       1360
+
+#define MOVE_NEUTRAL    0
+#define MOVE_FORWARD    1   
+#define MOVE_LEFT       2
+#define MOVE_RIGHT      3
+#define MOVE_BACK       4
+#define GOAL_FORWARD    5          //ゴール付近 ゆっくり
+#define GOAL_LEFT       6
+#define GOAL_RIGHT      7
+#define MAX_FORWARD     8          //はやい 姿勢修正用
+#define MAX_BACK        9
+
+PwmOut servoR(PC_6);
+PwmOut servoL(PC_7);
+
+void MoveCansat(int code);
+
+int main() {
+    while(1) {
+        //printf("Hello World!!");
+        MoveCansat(MOVE_NEUTRAL);
+        printf("STOP\r\n");
+        wait(2);
+        MoveCansat(MOVE_FORWARD);
+        printf("Move Forward\r\n");
+        wait(2);
+        MoveCansat(MOVE_RIGHT);
+        printf("Move right\r\n");
+        wait(2);
+        MoveCansat(MOVE_LEFT);
+        printf("Move left\r\n");
+        wait(2);
+        }
+}
+
+void MoveCansat(int code)
+{
+    switch(code){
+        case 0: //MOVE_NEUTRAL 
+            servoR.pulsewidth_us(servo_NEUTRAL_R);
+            servoL.pulsewidth_us(servo_NEUTRAL_L);
+            break;
+        case 1: //MOVE_FORWARD
+            servoR.pulsewidth_us(servo_FORWARD_R);
+            servoL.pulsewidth_us(servo_FORWARD_L);
+            break;
+        case 2: //MOVE_LEFT 
+            servoR.pulsewidth_us(servo_slow_FORWARD_R);
+            servoL.pulsewidth_us(servo_slow_back_L);
+        case 3: //MOVE_RIGHT 
+            servoR.pulsewidth_us(servo_slow_back_R);
+            servoL.pulsewidth_us(servo_slow_FORWARD_L);
+            break;
+        case 4: //MOVE_BACK 
+            servoR.pulsewidth_us(servo_back_R);
+            servoL.pulsewidth_us(servo_back_L);
+            break;
+        case 5: //GOAL_FORWARD
+            servoR.pulsewidth_us(servo_slow_FORWARD_R);
+            servoL.pulsewidth_us(servo_slow_FORWARD_L);
+            break;
+        case 6: //GOAL_LEFT 
+            servoR.pulsewidth_us(servo_slow_FORWARD_R);
+            servoL.pulsewidth_us(servo_slow_back_L);
+            break;
+        case 7: //GOAL_RIGHT 
+            servoR.pulsewidth_us(servo_slow_back_R);
+            servoL.pulsewidth_us(servo_slow_FORWARD_L);
+            break;
+        case 8: //MAX_FORWARD 
+            servoR.pulsewidth_us(servo_FORWARD_R);
+            servoL.pulsewidth_us(servo_FORWARD_L);
+            break;
+        case 9: //MAX_BACK 
+            servoR.pulsewidth_us(servo_back_R);
+            servoL.pulsewidth_us(servo_back_L);
+            break;
+    }
+    return;
+}
\ No newline at end of file