Diff: main.cpp

Revision:

16:c0a1daeb9fa5

Parent:

15:6ebca0a1aaca

Child:

17:4c96838e579f

--- a/main.cpp	Fri Jan 22 16:40:02 2016 +0000
+++ b/main.cpp	Fri Jan 22 19:23:59 2016 +0000
@@ -13,11 +13,14 @@
 float U_Kc;
 float U_Ti;
 float U_Td;
+float U_Ki=U_Kc*U_Ti;
+float U_Kd=U_Kc*U_Td;
 //-lower-//
 float L_Kc;
 float L_Ti;
 float L_Td;
-
+float L_Ki=L_Kc*L_Ti;
+float L_Kd=L_Kc*L_Td;
 //*****************************************************/
 // Global  //
 Ticker Recieve;
@@ -32,10 +35,10 @@
 uint8_t Height[4];
 uint8_t Wheelpos[4];
 uint8_t Mag[24];
-uint8_t Offset[4];
+uint8_t Offset[8];
 uint8_t Body_width[4];
-uint8_t Angle_Range_Up[4];
-uint8_t Angle_Range_Low[4];
+uint8_t Angle_Range_Up[8];
+uint8_t Angle_Range_Low[8];
 //-- encoder --
 float up_angle,low_angle;
 int Upper_Position;
@@ -299,57 +302,60 @@
                     }
                     case ANGLE_RANGE_UP: {
                         int i;
-                        for(i=0; i<4; i++) {
+                        for(i=0; i<8; i++) {
                             Angle_Range_Up[0+i]=cmd[1+i];
                         }
                     }
                     case ANGLE_RANGE_LOW: {
                         int i;
-                        for(i=0; i<4; i++) {
+                        for(i=0; i<8; i++) {
                             Angle_Range_Low[0+i]=cmd[1+i];
                         }
                     }
+                    // unfinish yet!!!!!!!!!!!!!!!!!
+                    case SAVE_EEPROM_DATA: {
+                        if (cmd[1]==0){
+                        
+                        }
+                    }
                 }
                 case READ_DATA: {
                     switch (cmd[0]) {
                         case MOTOR_UPPER_ANG: {
-
-                        }
-                        case MOTOR_LOWER_ANG: {
-
+                            com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);    
                         }
                         case UP_MARGIN: {
-
+                            com.sendUpMargin(MY_ID,UpMargin);                            
                         }
                         case LOW_MARGIN: {
-
+                            com.sendLowMargin(MY_ID,LowMargin);
                         }
                         case PID_UPPER_MOTOR: {
-
+                            com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd);
                         }
                         case PID_LOWER_MOTOR: {
-
+                            com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd);
                         }
                         case HEIGHT: {
-
+                            com.sendHeight(MY_ID,Height);
                         }
                         case WHEELPOS: {
-
+                            com.sendWheelPos(MY_ID,Wheelpos);
                         }
                         case MAG_DATA: {
-
+                            com.sendMagData(MY_ID,Mag);
                         }
                         case OFFSET: {
-
+                            com.sendOffset(MY_ID,Offset);
                         }
                         case BODY_WIDTH: {
-
+                            com.sendBodyWidth(MY_ID,Body_width);
                         }
                         case ANGLE_RANGE_UP: {
-
+                            com.sendUpAngleRange(MY_ID,Angle_Range_Up);
                         }
                         case ANGLE_RANGE_LOW: {
-
+                            com.sendLowAngleRange(MY_ID,Angle_Range_Low);
                         }
                     }
                 }
@@ -399,9 +405,6 @@
         Move_Upper();
         Move_Lower();
     }
-
-
-    //com.ReceiveCommand(id,data_array);
 }