Diff: main.cpp

Revision:

1:74aad7c18ccf

Parent:

0:65d0d8265089

--- a/main.cpp	Fri Nov 30 13:09:47 2018 +0000
+++ b/main.cpp	Fri Dec 21 02:25:54 2018 +0000
@@ -14,10 +14,16 @@
 #define dt          0.015f
 #define Task_1_NN   120
 #define Task_2_NN   5
+#define Task_4_NN   9
 
 //Structure
 #define Buff_size   16              //Serial Buffer
 
+//Initial Position
+#define init_leg_Angle1 0
+#define init_leg_length 0.22f
+#define init_leg_Angle2 0
+
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
@@ -41,10 +47,10 @@
 DigitalOut  LH_CS(PG_2);
 
 Serial      RF_Cmd(PG_14, PC_7);    //Serial_6
-DigitalOut  RF_CS(PA_5);
+DigitalOut  RF_CS(PG_9);
 
 Serial      RH_Cmd(PE_8, PE_7);     //Serial_7
-DigitalOut  RH_CS(PA_5);
+DigitalOut  RH_CS(PE_10);
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
 
 
@@ -70,11 +76,27 @@
 uint8_t Recieve_index = 0;
 
 
+//╔═════════════════╗
+//║   Leg Command   ║
+//╚═════════════════╝
+
+float   LF_q_0_E[3] = {            
+    init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
 float   LH_q_0_E[3] = {            
-    0, 0, 0
+    init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
+float   RF_q_0_E[3] = {            
+    init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
+float   RH_q_0_E[3] = {            
+    init_leg_Angle1, init_leg_length, init_leg_Angle2
 };
 
-int up = 0;
+uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
 
 
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
@@ -104,7 +126,7 @@
     LH_CS = 1;
     wait_ms(200);
     
-    LH_q_0_E[1] = 0.1;
+
     
     while(1) {                              //main() loop
 
@@ -117,28 +139,73 @@
 //        if(device.readable()) {
 //            pc.putc(device.getc());
 //        }
-        if(Flag_1 == 1) {               //check pending
-            switch (up) {
-                case 0:
-                    LH_q_0_E[1] = 0.17;
-                    break;
-                case 1:
-                    LH_q_0_E[1] = 0.23;
-                    break;
-            }
+        
+        //Send commands
+        if(Flag_4 == 1) {             
+            
+            LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255;
+            LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255;
+            LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255;
+            
+            LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255;
+            LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255;
+            LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255;
+            
+            RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255;
+            RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255;
+            RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255;
+            
+            RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255;
+            RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255;
+            RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255;
+            
+            //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]);
+            //LED
             LD1 = 1;
+            
+            //Left Front Leg
+            LF_CS = 0;
+            wait_us(20);
+            LF_Cmd.putc(LF_Cmd_Hex[0]);
+            LF_Cmd.putc(LF_Cmd_Hex[1]);
+            LF_Cmd.putc(LF_Cmd_Hex[2]);
+            LF_Cmd.putc('Q');
+            wait_us(180);
+            LF_CS = 1;
+            
+            //Left Hind Leg
             LH_CS = 0;
             wait_us(20);
-            LH_Cmd.putc(LH_q_0_E[0]);
-            LH_Cmd.putc(((LH_q_0_E[1]-0.1)/0.2)*255);
-            LH_Cmd.putc(LH_q_0_E[2]);
+            LH_Cmd.putc(LH_Cmd_Hex[0]);
+            LH_Cmd.putc(LH_Cmd_Hex[1]);
+            LH_Cmd.putc(LH_Cmd_Hex[2]);
             LH_Cmd.putc('Q');
             wait_us(180);
             LH_CS = 1;
+            
+            //Right Front Leg
+            RF_CS = 0;
+            wait_us(20);
+            RF_Cmd.putc(RF_Cmd_Hex[0]);
+            RF_Cmd.putc(RF_Cmd_Hex[1]);
+            RF_Cmd.putc(RF_Cmd_Hex[2]);
+            RF_Cmd.putc('Q');
+            wait_us(180);
+            RF_CS = 1;
+            
+            //Right Hind Leg
+            RH_CS = 0;
+            wait_us(20);
+            RH_Cmd.putc(RH_Cmd_Hex[0]);
+            RH_Cmd.putc(RH_Cmd_Hex[1]);
+            RH_Cmd.putc(RH_Cmd_Hex[2]);
+            RH_Cmd.putc('Q');
+            wait_us(180);
+            RH_CS = 1;
+            
             LD1 = 0;
-            //Task_2 done
             
-            Flag_1 = 0;                 //clear pending
+            Flag_4 = 0;                 //clear pending
         }     
     }
 }
@@ -156,7 +223,11 @@
     if(Task_1_count > Task_1_NN) {
         Task_1_count = 0;               //Task triggering
         Flag_1 = 1;
-        up = !up;
+    }
+    Task_4_count = Task_4_count + 1;
+    if(Task_4_count > Task_4_NN) {
+        Task_4_count = 0;               //Task triggering
+        Flag_4 = 1;
     }
 }
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//