Diff: main.cpp

Revision:

10:3b3d6bc88677

Parent:

7:bf239d051e8c

Child:

11:3dd92d1d542c

--- a/main.cpp	Sat Jan 16 03:25:18 2016 +0000
+++ b/main.cpp	Sat Jan 16 12:06:59 2016 +0000
@@ -10,19 +10,21 @@
 //*****************************************************/
 //--PID parameter--
 //-Upper-//
-int U_Kc;
-int U_Ti;
-int U_Td;
+float U_Kc;
+float U_Ti;
+float U_Td;
 //-lower-//
-int L_Kc;
-int L_Ti;
-int L_Td;
+float L_Kc;
+float L_Ti;
+float L_Td;
 
 //*****************************************************/
 // Global  //
 //-- Communication --
 Serial PC(D1,D0);
 ANDANTE_PROTOCOL_PACKET command;
+Bear_Receiver com(Tx,Rx,115200);
+#define MY_ID 0x01
 //-- encoder --
 int Upper_Position;
 int Lower_Position;
@@ -95,7 +97,7 @@
 {
     Lower.period(0.001);
     Low_PID.setMode(0);
-    Low_PID.setInputLimits(0,127);
+    Low_PID.setInputLimits(0,127); // set range
     Low_PID.setOutputLimits(0,1);
 }
 //******************************************************/
@@ -132,20 +134,75 @@
 }
 //******************************************************/
 
+void CmdCheck(uint8_t *cmd)
+{
+    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();
+        }
+
+        case GET_MOTOR_UPPER_ANG : {
+            float up_angle,low_angle;
+            //
+            com.sendMotorPos(MY_ID,up_angle,low_angle);
+        }
+        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;
+            
+            //
+        }
+    }
+}
+
 
 int main()
 {
+    uint8_t id;
+    uint8_t data_array[10];
     SET_UpperPID();
     SET_LowerPID();
-
-    while(1) {
-        Up_PID.setSetPoint(Upper_SetPoint);
-        Low_PID.setSetPoint(Lower_SetPoint);
+    /*
+        while(1) {
+            //Set Set_point
+            Up_PID.setSetPoint(Upper_SetPoint);
+            Low_PID.setSetPoint(Lower_SetPoint);
 
-        Move_Upper();
-        Move_Lower();
-    }
+            //Control Motor
+            Move_Upper();
+            Move_Lower();
+        }
+    */
 
+    com.ReceiveCommand(id,data_array);
 }