Diff: Movement.cpp

Revision:

0:6b57aea7ad9b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Movement.cpp	Mon Dec 06 08:54:49 2021 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "Movement.h"  
+
+//上から順にservo1.2.3.4.5.6の基準値　これを変えればすべての値が変わる
+#define S1 1.0
+#define S2 1.0
+#define S3 1.0
+#define S4 1.0
+#define S5 1.0
+#define S6 1.0
+
+
+//前と後ろ
+Servo servo1(D7);    
+Servo servo3(A1); 
+Servo servo4(D12);
+Servo servo6(A5); 
+
+//新しく取り付けるもの
+Servo servo2(A3);
+Servo servo5(D10);
+
+   void Movement::stop(){
+        servo1 = S1/2;
+        servo3 = S3/2;
+        servo4 = S4/2;
+        servo6 = S6/2;
+        
+        //
+        servo2 = S2/2;
+        servo5 = S5/2;
+        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 = S1;
+        servo2 = S2;
+        servo3 = S3;
+        servo4 = S4;
+        servo5 = S5;
+        servo6 = S6;
+        wait(5);
+    }
+ 
+//右に曲がる
+    void Movement::turn_right(int theta = 15)
+    {
+        servo1 = S1;
+        servo2 = S2;
+        servo3 = S3;
+        servo4 = 0;
+        servo5 = 0;
+        servo6 = 0;
+        wait(int(theta/15));
+    }
+ 
+//左に曲がる
+    void Movement::turn_left(int theta = 15)
+    {
+        servo1 = 0;
+        servo2 = 0;
+        servo3 = 0;
+        servo4 = S4;
+        servo5 = S5;
+        servo6 = S6;
+        wait(int(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