Sungwoo Kim
rainbow
Diff: CAN/function_CAN.cpp
- Revision:
- 56:6f50d9d3bfee
- Parent:
- 55:b25725257569
- Child:
- 57:f4819de54e7a
diff -r b25725257569 -r 6f50d9d3bfee CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp Wed Feb 26 12:51:52 2020 +0000
+++ b/CAN/function_CAN.cpp Tue Mar 03 12:27:53 2020 +0000
@@ -102,7 +102,10 @@
case CRX_SET_CAN_FREQ: {
CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
ROM_RESET_DATA();
+
+ //spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
break;
}
@@ -113,8 +116,7 @@
}
case CRX_SET_CONTROL_MODE: {
- //CONTROL_MODE = (int16_t) (msg.data[1]);
- CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
+ CONTROL_MODE = (int16_t) (msg.data[1]);
if (CONTROL_MODE == 22) { //MODE_FIND_HOME
FLAG_FIND_HOME = true;
}
@@ -228,8 +230,8 @@
}
case CRX_SET_HOMEPOS: {
- //CONTROL_MODE = 22;
- CONTROL_UTILITY_MODE = 22;
+ CONTROL_MODE = 22;
+ SETTING_SWITCH = (int16_t) (msg.data[1]);
break;
}
@@ -283,6 +285,10 @@
VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
ROM_RESET_DATA();
+
+ //spi_eeprom_write(RID_VALVE_CNETER, (int16_t) (VALVE_CENTER));
+ //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
+ //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
break;
}
@@ -443,9 +449,11 @@
}
case CRX_SET_VALVE_LIMIT: {
- VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
- VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
+ VALVE_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ VALVE_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
ROM_RESET_DATA();
+ //spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
+ //spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
break;
}
@@ -627,8 +635,9 @@
break;
}
case CRX_SET_VALVE_CENTER_OFFSET: {
- VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
- VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
+ VALVE_CENTER_OFFSET_times10 = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ VALVE_CENTER_OFFSET = ((float) VALVE_CENTER_OFFSET_times10) * 0.1f;
+ DDV_CENTER = DDV_CENTER + VALVE_CENTER_OFFSET;
ROM_RESET_DATA();
break;
}
@@ -703,37 +712,22 @@
if(address==CID_RX_CMD){
unsigned int CMD = msg.data[0];
ReadCMD(CMD);
-
} else if(address==CID_RX_REF_POSITION) {
-
+ //int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
+ //int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8);
- int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
- if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
- pos.ref = (double)temp_pos * 1.0f;
- vel.ref = (double)temp_vel * 10.0f;
- }
- else { //Linear Actuator
- pos.ref = (double)temp_pos * 4.0f;
- vel.ref = (double)temp_vel * 100.0f;
- }
-
+ int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
+ pos.ref = (double)temp_pos * 4.0f;
+ vel.ref = (double)temp_vel * 100.0f;
torq.ref = (double)temp_torq * 0.1f;
-
} else if(address==CID_RX_REF_VALVE_POS) {
int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
-
- if ((OPERATING_MODE&0b110) == 0) { //Moog Valve
- valve_pos.ref = (double) temp_ref_valve_pos;
- } else if ((OPERATING_MODE&0b110) == 1) { //KNR Valve
- valve_pos.ref = (double) temp_ref_valve_pos;
- } else { //SW Valve
- if(temp_ref_valve_pos >= 0) {
- valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
- } else {
- valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
- }
+ if(temp_ref_valve_pos >= 0){
+ valve_pos.ref = (double)DDV_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)DDV_CENTER)/10000.0f ;
+ }else{
+ valve_pos.ref = (double)DDV_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)DDV_CENTER)/10000.0f;
}
} else if(address==CID_RX_REF_PWM){
int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
@@ -755,7 +749,7 @@
temp_msg.data[2] = (uint8_t) CAN_FREQ;
temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8);
temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
- temp_msg.data[5] = (uint8_t) CONTROL_UTILITY_MODE;
+ temp_msg.data[5] = (uint8_t) CONTROL_MODE;
temp_msg.data[6] = (uint8_t) OPERATING_MODE;
can.write(temp_msg);
@@ -804,7 +798,7 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
- temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
+ temp_msg.data[1] = (uint8_t) CONTROL_MODE;
can.write(temp_msg);
}
@@ -920,8 +914,8 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
- temp_msg.data[1] = (uint8_t) VALVE_CENTER;
- temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
+ temp_msg.data[1] = (uint8_t) (int) (VALVE_CENTER);
+ temp_msg.data[2] = (uint8_t) ((int) (VALVE_CENTER) >> 8);
temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS);
temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS);
@@ -1052,10 +1046,10 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
- temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
- temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
- temp_msg.data[3] = (uint8_t) VALVE_MAX_POS;
- temp_msg.data[4] = (uint8_t) (VALVE_MAX_POS >> 8);
+ temp_msg.data[1] = (uint8_t) VALVE_LIMIT_MINUS;
+ temp_msg.data[2] = (uint8_t) (VALVE_LIMIT_MINUS >> 8);
+ temp_msg.data[3] = (uint8_t) VALVE_LIMIT_PLUS;
+ temp_msg.data[4] = (uint8_t) (VALVE_LIMIT_PLUS >> 8);
can.write(temp_msg);
}
@@ -1135,8 +1129,8 @@
float temp_valve_deadzone_plus = 0.0f;
float temp_ddv_center = 0.0f;
- temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
- temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+ temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+ temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
temp_ddv_center = 0.0f;
temp_msg.id = CID_TX_INFO;
@@ -1215,10 +1209,10 @@
int16_t valve_pos_vs_pwm;
// valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
- if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
- valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
+ if(VALVE_POS_VS_PWM[canindex]>=DDV_CENTER) {
+ valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
} else {
- valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+ valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
}
int16_t PWM_VALVE_ID;
@@ -1240,12 +1234,12 @@
int32_t valve_pos_vs_flowrate;
valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
- int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + VALVE_CENTER;
+ int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + (int) DDV_CENTER;
int16_t temp_valve_pos = 0;
- if(VALVE_POS_VALVE_ID>=VALVE_CENTER) {
- temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER));
+ if(VALVE_POS_VALVE_ID>=DDV_CENTER) {
+ temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER));
} else {
- temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER));
+ temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER));
}
temp_msg.id = CID_TX_INFO;
@@ -1351,7 +1345,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_PRES;
- temp_msg.len = 4;
+ temp_msg.len = 8;
temp_msg.data[0] = (uint8_t) t_pres_a;
temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
temp_msg.data[2] = (uint8_t) t_pres_b;
@@ -1363,8 +1357,8 @@
void CAN_TX_PWM(int16_t t_pwm) {
CANMessage temp_msg;
- temp_msg.id = CID_TX_VOUT;
- temp_msg.len = 2;
+ temp_msg.id = CID_TX_POSITION;
+ temp_msg.len = 8;
temp_msg.data[0] = (uint8_t) t_pwm;
temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
@@ -1395,4 +1389,3 @@
-