Update CoordsControl.ino

Author: wxy6655

Arduino/MycobotBasic/examples/CoordsControl/CoordsControl.ino

@@ -7,7 +7,16 @@ int power_pin = 2;
 int open_pin = 5;
 int out_state = 1;
 
-Coords pos_init_1 = {155.10, -55.10, 90.10, -179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_1 = {155.10, -55.10, 90.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_2 = {255.10, -55.10, 90.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_3 = {255.10,  55.10, 90.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_4 = {155.10,  55.10, 90.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_5 = {155.10, -55.10, 90.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_6 = {155.10, -55.10, 150.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_7 = {155.10,  55.10, 150.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_8 = {255.10,  55.10, 150.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_9 = {255.10, -55.10, 150.10, 179.10, 0.10, -90.10};   // 初始坐标
+Coords pos_init_10 = {155.10, -55.10, 150.10, 179.10, 0.10, -90.10};   // 初始坐标
 Coords data1;
 Angles angles_0 = {0, 0, 0, 0, 0, 0};             //初始化角度
 
@@ -28,7 +37,7 @@ void setup() {              //初始化函数
   delay(3000);
   Serial.printf("A键控制力矩开关         |");
   Serial.println("");
-  Serial.printf("B键控制移动到初始坐标    |");
+  Serial.printf("B键控制立方体移动       |");
   Serial.println("");
   Serial.printf("C键控制前后左右上下移动  |");
   Serial.println("");
@@ -43,9 +52,30 @@ void loop() {               //主函数
       reless();
       } 
     if (M5.BtnB.wasReleased()) {        //移动到初始角度
-      myCobot.writeCoords(pos_init_1,30);
+      myCobot.writeCoords(pos_init_1,50);
+      delay(3000);
+      myCobot.writeCoords(pos_init_2,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_3,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_4,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_5,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_6,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_7,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_8,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_9,30);
+      delay(2000);
+      myCobot.writeCoords(pos_init_10,30);
+      delay(2000);
     }
     if (M5.BtnC.wasReleased()) {        //前后左右上下移动
+      myCobot.writeCoords(pos_init_1,30);
+      delay(3000);
       data1 = pos_init_1;
       delay(50);
       move_coords(0, 10);
@@ -60,6 +90,18 @@ void loop() {               //主函数
       delay(2000);
       move_coords(2, -10);
       delay(2000);
+      move_coords(3, -10);
+      delay(2000);
+      move_coords(3, 10);
+      delay(2000);
+      move_coords(4, -10);
+      delay(2000);
+      move_coords(4, 10);
+      delay(2000);
+      move_coords(5, -10);
+      delay(2000);
+      move_coords(5, 10);
+      delay(2000);
 }
 }
 
@@ -102,19 +144,19 @@ void move_coords(int dir, int num)    //控制移动方向和步数，一步等
 {
   if (num > 0){
     for(int i = 0; i<num; i++){
-    myCobot.writeCoords(data1,15);
-    for(int i=0; i<6; i++){
-      Serial.print(data1[i]);
-      Serial.print(", ");
-    }
-    Serial.println("");
-    data1[dir] += 10;
-    delay(100);
+      myCobot.writeCoords(data1,30);
+      for(int i=0; i<6; i++){
+        Serial.print(data1[i]);
+        Serial.print(", ");
+      }
+      Serial.println("");
+      data1[dir] += 10;
+      delay(100);
   }
   }else{
     num = 0-num;
     for(int i = 0; i<num; i++){
-    myCobot.writeCoords(data1,15);
+    myCobot.writeCoords(data1,30);
     for(int i=0; i<6; i++){
       Serial.print(data1[i]);
       Serial.print(", ");