Diff: main.cpp

Revision:

18:face01c94152

Parent:

17:4c96838e579f

Child:

19:3da61e637b2d

--- a/main.cpp	Sun Jan 24 07:57:55 2016 +0000
+++ b/main.cpp	Sun Jan 24 08:26:35 2016 +0000
@@ -154,12 +154,14 @@
     if(id==MY_ID) {
         switch (ins) {
             case PING: {
-
+                break;
             }
             case WRITE_DATA: {
                 switch (cmd[0]) {
                     case ID: {
-                        MY_ID = cmd[1];
+                        ///
+                        MY_ID = (int16_t)cmd[1];
+                        break;
                     }
                     case MOTOR_UPPER_ANG: {
                         uint8_t IntUpAngle[2],FloatUpAngle[2];
@@ -180,19 +182,22 @@
                         FloatLowAngle[1]=cmd[8];
                         int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
                         float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
-                        low_angle=int_buffer+float_buffer;;
+                        low_angle=int_buffer+float_buffer;
+                        break;
                     }
                     case UP_MARGIN: {
                         int i;
                         for(i=0; i<4; i++) {
                             UpMargin[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case LOW_MARGIN: {
                         int i;
                         for(i=0; i<4; i++) {
                             LowMargin[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case KP_UPPER_MOTOR: {
                         uint8_t int_buffer[2];
@@ -205,6 +210,7 @@
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         U_Kc=Int+Float;
+                        break;
                     }
                     case KI_UPPER_MOTOR: {
                         uint8_t int_buffer[2];
@@ -218,6 +224,7 @@
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         KI=Int+Float;
                         U_Ti=KI/U_Kc;
+                        break;
                     }
                     case KD_UPPER_MOTOR: {
                         uint8_t int_buffer[2];
@@ -231,6 +238,7 @@
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         KD=Int+Float;
                         U_Td=KD/U_Kc;
+                        break;
                     }
                     case KP_LOWER_MOTOR: {
                         uint8_t int_buffer[2];
@@ -243,6 +251,7 @@
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         L_Kc=Int+Float;
+                        break;
                     }
                     case KI_LOWER_MOTOR: {
                         uint8_t int_buffer[2];
@@ -256,6 +265,7 @@
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         KI=Int+Float;
                         L_Ti=KI/L_Kc;
+                        break;
                     }
                     case KD_LOWER_MOTOR: {
                         uint8_t int_buffer[2];
@@ -269,94 +279,120 @@
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         KD=Int+Float;
                         L_Td=KD/L_Kc;
+                        break;
                     }
                     case HEIGHT: {
                         int i;
                         for(i=0; i<4; i++) {
                             Height[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case WHEELPOS: {
                         int i;
                         for(i=0; i<4; i++) {
                             Wheelpos[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case MAG_DATA: {
                         int i;
                         for(i=0; i<24; i++) {
                             Mag[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case OFFSET: {
                         int i;
                         for(i=0; i<8; i++) {
                             Offset[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case BODY_WIDTH: {
                         int i;
                         for(i=0; i<4; i++) {
                             Body_width[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case ANGLE_RANGE_UP: {
                         int i;
                         for(i=0; i<8; i++) {
                             Angle_Range_Up[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     case ANGLE_RANGE_LOW: {
                         int i;
                         for(i=0; i<8; i++) {
                             Angle_Range_Low[0+i]=cmd[1+i];
                         }
+                        break;
                     }
                     // unfinish yet!!!!!!!!!!!!!!!!!
                     case SAVE_EEPROM_DATA: {
-                        if (cmd[1]==ID){
-                        
+                        if (cmd[1]==ID) {
+
                         }
+                        break;
                     }
+
+                    break;
                 }
                 case READ_DATA: {
                     switch (cmd[0]) {
                         case MOTOR_UPPER_ANG: {
-                            com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);    
+                            uint8_t status;
+                            status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
+                            printf("status = 0x%02x\n\r",status);
+                            break;
                         }
                         case UP_MARGIN: {
-                            com.sendUpMargin(MY_ID,UpMargin);                            
+                            com.sendUpMargin(MY_ID,UpMargin);
+                            break;
                         }
                         case LOW_MARGIN: {
                             com.sendLowMargin(MY_ID,LowMargin);
+                            break;
                         }
                         case PID_UPPER_MOTOR: {
                             com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd);
+                            break;
                         }
                         case PID_LOWER_MOTOR: {
                             com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd);
+                            break;
                         }
                         case HEIGHT: {
                             com.sendHeight(MY_ID,Height);
+                            break;
                         }
                         case WHEELPOS: {
                             com.sendWheelPos(MY_ID,Wheelpos);
+                            break;
                         }
                         case MAG_DATA: {
                             com.sendMagData(MY_ID,Mag);
+                            break;
                         }
                         case OFFSET: {
                             com.sendOffset(MY_ID,Offset);
+                            break;
                         }
                         case BODY_WIDTH: {
                             com.sendBodyWidth(MY_ID,Body_width);
+                            break;
                         }
                         case ANGLE_RANGE_UP: {
                             com.sendUpAngleRange(MY_ID,Angle_Range_Up);
+                            break;
                         }
                         case ANGLE_RANGE_LOW: {
                             com.sendLowAngleRange(MY_ID,Angle_Range_Low);
+                            break;
                         }
+                        break;
                     }
                 }
             }
@@ -385,17 +421,19 @@
     uint8_t data_array[30];
     uint8_t id;
     uint8_t ins;
+    uint8_t status;
 
-    com.ReceiveCommand(&id,data_array,&ins);
-    CmdCheck((int16_t)id,data_array,ins);
+    status = com.ReceiveCommand(&id,data_array,&ins);
+    if(status == ANDANTE_ERRBIT_NONE) {
+        CmdCheck((int16_t)id,data_array,ins);
+    }
 }
 /*******************************************************/
 int main()
 {
-    while(1)
-    {
+    while(1) {
         Rc();
-        }
+    }
     /*Recieve.attach(&Rc,0.000001);
     Start_Up();
     SET_UpperPID();