Diff: Movement.cpp

Revision:

8:7209c810309d

diff -r 74994694ec04 -r 7209c810309d Movement.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Movement.cpp	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "Movement.h"  
+
+Servo servo1(D7);    
+Servo servo2(A3);    
+Servo servo3(A1); 
+Servo servo4(D12);
+Servo servo5(D10);
+Servo servo6(A5); 
+
+   void Movement::stop(){
+        servo1 = 0.5;
+        servo2 = 0.5;
+        servo3 = 0.5;
+        servo4 = 0.5;
+        servo5 = 0.5;
+        servo6 = 0.5;
+        wait(1);
+    }
+ 
+//前進
+    void Movement::move_forward(int time = 20)
+    {
+        servo1 = 0;
+        servo2 = 0;
+        servo3 = 0;
+        servo4 = 0;
+        servo5 = 0;
+        servo6 = 0;
+        wait(time);
+    }
+ 
+//後退
+    void Movement::move_backward()
+    {
+        servo1 = 1;
+        servo2 = 1;
+        servo3 = 1;
+        servo4 = 1;
+        servo5 = 1;
+        servo6 = 1;
+        wait(5);
+    }
+ 
+//右に曲がる
+    void Movement::turn_right(int theta = 15)
+    {
+        servo1 = 1;
+        servo2 = 1;
+        servo3 = 1;
+        servo4 = 0;
+        servo5 = 0;
+        servo6 = 0;
+        wait(theta/15);
+    }
+ 
+//左に曲がる
+    void Movement::turn_left(int theta = 15)
+    {
+        servo1 = 0;
+        servo2 = 0;
+        servo3 = 0;
+        servo4 = 1;
+        servo5 = 1;
+        servo6 = 1;
+        wait(theta/15);
+    }
+ 
+//倒れているときの処理
+void Movement::wakeup(int time){
+        for(int i=1;i<=time;i++)
+        {
+        move_forward(5);
+        move_backward();
+        turn_right();
+        turn_left();
+    }
+}    
\ No newline at end of file