Revision 0:f4186a5283c3, committed 2021-12-09

Comitter:

minanao

Date:

Thu Dec 09 04:48:32 2021 +0000

Commit message:

For MG996R

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Movement.cpp	Thu Dec 09 04:48:32 2021 +0000
@@ -0,0 +1,137 @@
+
+#include "mbed.h"
+#include "Movement.h" 
+//for MG996R Movement class
+//timeはmsで指定 指定しない場合5s回転
+DigitalOut servo1(D7);    
+DigitalOut servo2(A3);    
+DigitalOut servo3(A1); 
+DigitalOut servo4(D12);
+DigitalOut servo5(D10);
+DigitalOut servo6(A5); 
+#define PWMoff 20 // Waiting for end PWD
+#define Go   1 // Move to Right -> DESTRA   or DX
+#define Back    5 // Move to LEFT  <- SINISTRA or SX
+#define DLY     1
+#define Stop 0
+ 
+
+
+void Movement::Move(double ON,DigitalOut& Drive,int time=250)
+{ 
+int n,num;
+num=int(time/20);
+for(n=0; n<num; n++)
+    {
+    
+        Drive=1;
+        wait_ms(ON);
+        Drive=0;
+        wait_ms(20);
+    }
+ 
+}
+
+
+   void Movement::stop(){
+        Move(Stop,servo1);
+        Move(Stop,servo2);
+        Move(Stop,servo3);
+        Move(Stop,servo4);
+        Move(Stop,servo5);
+        Move(Stop,servo6);
+        wait(1);
+    }
+ 
+//前進
+    void Movement::move_forward(int time = 250)
+    {
+        Move(Go,servo1,time);
+        Move(Go,servo2,time);
+        Move(Go,servo3,time);
+        Move(Go,servo4,time);
+        Move(Go,servo5,time);
+        Move(Go,servo6,time);
+       
+        
+    }
+ 
+//後退
+    void Movement::move_backward()
+    {
+        Move(Back,servo1);
+        Move(Back,servo2);
+        Move(Back,servo3);
+        Move(Back,servo4);
+        Move(Back,servo5);
+        Move(Back,servo6);
+        
+    }
+ 
+//右に曲がる
+    void Movement::turn_right(int theta = 15)
+    {
+        Move(Go,servo1,int(theta/15));
+        Move(Go,servo2,int(theta/15));
+        Move(Go,servo3,int(theta/15));
+        Move(Stop,servo4,int(theta/15));
+        Move(Stop,servo5,int(theta/15));
+        Move(Stop,servo6,int(theta/15));
+        
+        wait(theta/15);
+    }
+ 
+//左に曲がる
+    void Movement::turn_left(int theta = 15)
+    {
+        Move(Stop,servo1,int(theta/15));
+        Move(Stop,servo2,int(theta/15));
+        Move(Stop,servo3,int(theta/15));
+        Move(Go,servo4,int(theta/15));
+        Move(Go,servo5,int(theta/15));
+        Move(Go,servo6,int(theta/15));
+        
+        wait(theta/15);
+    }
+ 
+//倒れているときの処理
+void Movement::wakeup(int time){
+        for(int i=1;i<=time;i++)
+        {
+        move_forward(5);
+        move_backward();
+        turn_right();
+        turn_left();
+    }
+}   
+/**
+int main() 
+{  
+int n=0;
+ 
+    printf("By: www.emcu.eu - Ver.1.0 \n\r");
+ 
+    printf("Rotate all to -> \n\r");
+    // Sposta tutto a DX 
+    for(n=0; n<20; n++)
+        Move(Right);
+ 
+    wait(DLY);
+ 
+    printf("Rotate all to <- \n\r");
+    // Sposta tutto a SX 
+    for(n=0; n<20; n++)
+        Move(Left);
+    
+    wait(DLY);
+    
+    printf("Move to 10 times to ->\n\r");
+    for(n=0; n<10; n++)
+        Move(Right);
+    
+    wait(DLY);
+    
+    printf("END \n\r\n\r");
+}
+**/
+ 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Movement.h	Thu Dec 09 04:48:32 2021 +0000
@@ -0,0 +1,26 @@
+#ifndef MOVEMENT_H
+#define MOVEMENT_H
+
+#include "mbed.h"
+  
+
+class Movement
+{    
+public:
+    void Move(double ON,DigitalOut& Drive,int time);
+
+//停止
+    void stop();
+//前進
+    void move_forward(int time);
+//後退
+    void move_backward();
+//右に曲がる
+    void turn_right(int theta);
+//左に曲がる
+    void turn_left(int theta);
+//横転から復帰    
+    void wakeup(int time);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Dec 09 04:48:32 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file