Diff: main.cpp

Revision:

12:6b3b997dd7c2

Parent:

11:3dd92d1d542c

Child:

13:49cb002ad8fd

--- a/main.cpp	Thu Jan 21 14:49:40 2016 +0000
+++ b/main.cpp	Thu Jan 21 16:27:04 2016 +0000
@@ -138,51 +138,55 @@
 
 void CmdCheck(uint8_t *cmd,uint8_t ins)
 {
-    switch(cmd[0]) {
-        case SET_MOTOR_UPPER_ANG : {
-            uint8_t Upper_setpoint_buffer[2];
-            uint8_t Lower_setpoint_buffer[2];
-            Upper_setpoint_buffer[0]=cmd[1];
-            Upper_setpoint_buffer[1]=cmd[2];
-            Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer);
-            Lower_setpoint_buffer[0]=cmd[5];
-            Lower_setpoint_buffer[1]=cmd[6];
-            Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer);
-            //Set Set_point
-            Up_PID.setSetPoint(Upper_SetPoint);
-            Low_PID.setSetPoint(Lower_SetPoint);
-
-            //Control Motor
-            Move_Upper();
-            Move_Lower();
+    switch (ins) {
+        case PING:{
+            
         }
-
-        case GET_MOTOR_UPPER_ANG : {
-            float up_angle,low_angle;
-            //
-            com.sendMotorPos(MY_ID,up_angle,low_angle);
+        case READ_DATA:{
+            switch (cmd[0]){
+                case ID:{
+                    MY_ID = cmd[1];
+                }
+                case MOTOR_UPPER_ANG:{
+                    uint8_t Upper_setpoint_buffer[2];
+                    Upper_setpoint_buffer[0]=cmd[1];
+                    Upper_setpoint_buffer[1]=cmd[2];
+                    Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer);
+                }
+                case MOTOR_LOWER_ANG:{
+                    uint8_t Lower_setpoint_buffer[2];
+                    Lower_setpoint_buffer[0]=cmd[1];
+                    Lower_setpoint_buffer[1]=cmd[2];
+                    Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer);
+                }
+                case MARGIN:{
+                    uint8_t Margin_buffer[4];
+                    Margin_buffer[0]=cmd[1];
+                    Margin_buffer[1]=cmd[2];
+                    Margin_buffer[2]=cmd[3];
+                    Margin_buffer[3]=cmd[4];
+                    Margin=Utilities::ConvertUInt8ArrayToInt16(Margin_buffer);
+                }
+                case KP_UPPER_MOTOR:{
+                    uint8_t KP_buffer[2];
+                    KP_buffer[0]=cmd[1];
+                    KP_buffer[1]=cmd[2];
+                    U_Kc=Utilities::ConvertUInt8ArrayToInt16(KP_buffer);
+                }
+                case KI_UPPER_MOTOR:{
+                    uint8_t KI_buffer[2];
+                    float KI;
+                    KI_buffer[0]=cmd[1];
+                    KI_buffer[1]=cmd[2];
+                    KI=Utilities::ConvertUInt8ArrayToInt16(KI_buffer);
+                    U_Ti=KI/U_Kc;
+                }
+                case KD_UPPER_MOTOR:{
+                    
+                    
+            
         }
-        case SAVE_DATA_TO_EEPROM : {
-            uint8_t Int_data_buffer[2];
-            uint8_t Float_data_buffer[2];
-            uint8_t Address_buffer[2];
-            float int_buffer;
-            float float_buffer;
-            float data;
-            uint16_t address;
-            Int_data_buffer[0]=cmd[1];
-            Int_data_buffer[1]=cmd[2];
-            Float_data_buffer[0]=cmd[3];
-            Float_data_buffer[1]=cmd[4];
-            int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Int_data_buffer);
-            float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Float_data_buffer)/FLOAT_CONVERTER;
-            
-            address=Utilities::ConvertUInt8ArrayToInt16(Address_buffer);
-            data=int_buffer+float_buffer;
-            
-            //
-        }
-    }
+    }    
 }
 /******************************************************/
 void Start_Up ()// load parameter from eeprom before start
@@ -205,7 +209,7 @@
     uint8_t data_array[10];
     int8_t ins;
     
-    com.ReceiveCommand(MY_ID,data_array);
+    com.ReceiveCommand(&MY_ID,data_array,&ins);
     CmdCheck(data_array,ins);
 }
 /*******************************************************/