Induk Choi
MCP with wheel
Diff: main.cpp
- Revision:
- 11:58a70b97c750
- Parent:
- 10:54e95428798b
- Child:
- 12:2aa99d54af80
diff -r 54e95428798b -r 58a70b97c750 main.cpp
--- a/main.cpp Thu Jun 04 01:26:01 2020 +0000
+++ b/main.cpp Mon Oct 19 23:29:46 2020 +0000
@@ -28,7 +28,10 @@
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
-
+ #define P_GAIN_MIN 0.0f
+ #define P_GAIN_MAX 2048.0f
+ #define I_GAIN_MIN 0.0f
+ #define I_GAIN_MAX 2048.0f
/// Joint Soft Stops ///
#define A_LIM_P 1.5f
#define A_LIM_N -1.5f
@@ -132,6 +135,19 @@
msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
msg->data[7] = t_int&0xff;
}
+
+void pack_cmd2(CANMessage * msg, joint_control joint){
+
+ /// limit data to be within bounds ///
+ float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
+ /// pack ints into the can buffer ///
+ uint16_t kp = (uint16_t)(fminf(fmaxf(P_GAIN_MIN, joint.kp), P_GAIN_MAX) * 32);
+ uint16_t ki = (uint16_t)(fminf(fmaxf(I_GAIN_MIN, joint.kd), I_GAIN_MAX) * 32);
+
+ memcpy(msg->data, &v_des, 4);
+ memcpy(msg->data + 4, &kp, 2);
+ memcpy(msg->data + 6, &ki, 2);
+ }
/// CAN Reply Packet Structure ///
/// 16 bit position, between -4*pi and 4*pi
@@ -147,31 +163,38 @@
void unpack_reply(CANMessage msg, leg_state * leg){
/// unpack ints from can buffer ///
- uint16_t id = msg.data[0];
- uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
- uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
- uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
- /// convert uints to floats ///
- float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
- float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
- float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
-
- if(id==1){
- leg->a.p = p;
- leg->a.v = v;
- leg->a.t = t;
+ if(msg.id == 6) {
+ float p, v;
+ memcpy(&v, msg.data, 4);
+ memcpy(&p, msg.data + 4, 4);
+ leg->k.v = v;
+ leg->k.p = p;
+ leg->k.t = 0.0f;
+ } else {
+ uint16_t id = msg.data[0];
+ uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
+ uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
+ uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
+ /// convert uints to floats ///
+ float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
+ float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
+ float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
+
+ if(id == 1) {
+ leg->a.p = p;
+ leg->a.v = v;
+ leg->a.t = t;
+ } else if(id == 2) {
+ leg->h.p = p;
+ leg->h.v = v;
+ leg->h.t = t;
+ } else if(id == 3) {
+ leg->k.p = p;
+ leg->k.v = v;
+ leg->k.t = t;
}
- else if(id==2){
- leg->h.p = p;
- leg->h.v = v;
- leg->h.t = t;
- }
- else if(id==3){
- leg->k.p = p;
- leg->k.v = v;
- leg->k.t = t;
- }
- }
+ }
+}
void rxISR1() {
can1.read(rxMsg1); // read message into Rx message storage
@@ -182,20 +205,22 @@
unpack_reply(rxMsg2, &l2_state);
}
void PackAll(){
- pack_cmd(&a1_can, l1_control.a);
- pack_cmd(&a2_can, l2_control.a);
+// pack_cmd(&a1_can, l1_control.a);
+// pack_cmd(&a2_can, l2_control.a);
pack_cmd(&h1_can, l1_control.h);
pack_cmd(&h2_can, l2_control.h);
- pack_cmd(&k1_can, l1_control.k);
- pack_cmd(&k2_can, l2_control.k);
+ pack_cmd2(&k1_can, l1_control.k);
+ pack_cmd2(&k2_can, l2_control.k);
}
void WriteAll(){
//toggle = 1;
+/*
can1.write(a1_can);
wait(.00002);
can2.write(a2_can);
wait(.00002);
+*/
can1.write(h1_can);
wait(.00002);
can2.write(h2_can);
@@ -251,7 +276,7 @@
msg->data[5] = 0xFF;
msg->data[6] = 0xFF;
msg->data[7] = 0xFE;
- WriteAll();
+ //WriteAll();
}
void EnterMotorMode(CANMessage * msg){
@@ -265,6 +290,18 @@
msg->data[7] = 0xFC;
//WriteAll();
}
+
+void EnterVelCtrlMode(CANMessage * msg){
+ msg->data[0] = 0xFF;
+ msg->data[1] = 0xFF;
+ msg->data[2] = 0xFF;
+ msg->data[3] = 0xFF;
+ msg->data[4] = 0xFF;
+ msg->data[5] = 0xFF;
+ msg->data[6] = 0xFF;
+ msg->data[7] = 0xFB;
+ //WriteAll();
+ }
void ExitMotorMode(CANMessage * msg){
msg->data[0] = 0xFF;
@@ -277,6 +314,7 @@
msg->data[7] = 0xFD;
//WriteAll();
}
+
void serial_isr(){
/// handle keyboard commands from the serial terminal ///
while(pc.readable()){
@@ -286,24 +324,26 @@
case(27):
//loop.detach();
printf("\n\r exiting motor mode \n\r");
- ExitMotorMode(&a1_can);
- ExitMotorMode(&a2_can);
+// ExitMotorMode(&a1_can);
+// ExitMotorMode(&a2_can);
ExitMotorMode(&h1_can);
ExitMotorMode(&h2_can);
ExitMotorMode(&k1_can);
ExitMotorMode(&k2_can);
+ l1_control.k.v_des = 0.0f;
+ l2_control.k.v_des = 0.0f;
enabled = 0;
break;
case('m'):
printf("\n\r entering motor mode \n\r");
- EnterMotorMode(&a1_can);
- EnterMotorMode(&a2_can);
+// EnterMotorMode(&a1_can);
+// EnterMotorMode(&a2_can);
EnterMotorMode(&h1_can);
EnterMotorMode(&h2_can);
- EnterMotorMode(&k1_can);
- EnterMotorMode(&k2_can);
+ EnterVelCtrlMode(&k1_can);
+ EnterVelCtrlMode(&k2_can);
wait(.5);
- enabled = 1;
+ enabled = 0xC;
//loop.attach(&sendCMD, .001);
break;
case('s'):
@@ -314,12 +354,32 @@
break;
case('z'):
printf("\n\r zeroing \n\r");
- Zero(&a1_can);
- Zero(&a2_can);
+// Zero(&a1_can);
+// Zero(&a2_can);
+ Zero(&h1_can);
+ Zero(&h2_can);
+ l1_control.k.v_des = 0.0f;
+ l2_control.k.v_des = 0.0f;
+ pack_cmd2(&k1_can, l1_control.k);
+ pack_cmd2(&k2_can, l2_control.k);
+ break;
+ case('+'):
+ printf("+");
Zero(&h1_can);
Zero(&h2_can);
- Zero(&k1_can);
- Zero(&k2_can);
+ l1_control.k.v_des += PI * 2.0f * 0.1f;
+ l2_control.k.v_des += PI * 2.0f * 0.1f;
+ pack_cmd2(&k1_can, l1_control.k);
+ pack_cmd2(&k2_can, l2_control.k);
+ break;
+ case('-'):
+ printf("-");
+ Zero(&h1_can);
+ Zero(&h2_can);
+ l1_control.k.v_des -= PI * 2.0f * 0.1f;
+ l2_control.k.v_des -= PI * 2.0f * 0.1f;
+ pack_cmd2(&k1_can, l1_control.k);
+ pack_cmd2(&k2_can, l2_control.k);
break;
}
}
@@ -370,7 +430,6 @@
PackAll();
WriteAll();
-
//for (int i = 0; i<TX_LEN; i++) {
// tx_buff[i] = 2*rx_buff[i];
//}
@@ -404,53 +463,74 @@
void control()
{
-
- if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){
- enabled = 1;
- EnterMotorMode(&a1_can);
- can1.write(a1_can);
- EnterMotorMode(&a2_can);
- can2.write(a2_can);
- EnterMotorMode(&k1_can);
- can1.write(k1_can);
- EnterMotorMode(&k2_can);
- can2.write(k2_can);
+ int32_t en_flag = spi_command.flags[0];
+
+ if((en_flag&0x1) && !(enabled&0x1)) {
+ enabled |= 0x1;
EnterMotorMode(&h1_can);
can1.write(h1_can);
+ }
+
+ if((en_flag&0x2) && !(enabled&0x2)) {
+ enabled |= 0x2;
EnterMotorMode(&h2_can);
can2.write(h2_can);
- printf("e\n\r");
- return;
+ wait(.00002);
+ }
+
+ if((en_flag&0x4) && !(enabled&0x4)) {
+ enabled |= 0x4;
+ EnterVelCtrlMode(&k1_can);
+ can1.write(k1_can);
+ wait(.00002);
}
- else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){
- enabled = 0;
- ExitMotorMode(&a1_can);
- can1.write(a1_can);
- ExitMotorMode(&a2_can);
- can2.write(a2_can);
+
+ if((en_flag&0x8) && !(enabled&0x8)) {
+ enabled |= 0x8;
+ EnterVelCtrlMode(&k2_can);
+ can2.write(k2_can);
+ wait(.00002);
+ }
+
+ if(!(en_flag&0x1) && (enabled&0x1)) {
+ enabled &= ~0x1;
ExitMotorMode(&h1_can);
can1.write(h1_can);
+ wait(.00002);
+ }
+
+ if(!(en_flag&0x2) && (enabled&0x2)) {
+ enabled &= ~0x2;
ExitMotorMode(&h2_can);
can2.write(h2_can);
+ wait(.00002);
+ }
+
+ if(!(en_flag&0x4) && (enabled&0x4)) {
+ enabled &= ~0x4;
ExitMotorMode(&k1_can);
can1.write(k1_can);
+ wait(.00002);
+ }
+
+ if(!(en_flag&0x8) && (enabled&0x8)) {
+ enabled &= ~0x8;
ExitMotorMode(&k2_can);
can2.write(k2_can);
- printf("x\n\r");
- return;
- }
-
- spi_data.q_abad[0] = l1_state.a.p;
+ wait(.00002);
+ }
+
+// spi_data.q_abad[0] = l1_state.a.p;
spi_data.q_hip[0] = l1_state.h.p;
spi_data.q_knee[0] = l1_state.k.p;
- spi_data.qd_abad[0] = l1_state.a.v;
+// spi_data.qd_abad[0] = l1_state.a.v;
spi_data.qd_hip[0] = l1_state.h.v;
spi_data.qd_knee[0] = l1_state.k.v;
- spi_data.q_abad[1] = l2_state.a.p;
+// spi_data.q_abad[1] = l2_state.a.p;
spi_data.q_hip[1] = l2_state.h.p;
spi_data.q_knee[1] = l2_state.k.p;
- spi_data.qd_abad[1] = l2_state.a.v;
+// spi_data.qd_abad[1] = l2_state.a.v;
spi_data.qd_hip[1] = l2_state.h.v;
spi_data.qd_knee[1] = l2_state.k.v;
@@ -470,51 +550,51 @@
memset(&l1_control, 0, sizeof(l1_control));
memset(&l2_control, 0, sizeof(l2_control));
-
- l1_control.a.p_des = spi_command.q_des_abad[0];
- l1_control.a.v_des = spi_command.qd_des_abad[0];
- l1_control.a.kp = spi_command.kp_abad[0];
- l1_control.a.kd = spi_command.kd_abad[0];
- l1_control.a.t_ff = spi_command.tau_abad_ff[0];
-
+
+// l1_control.a.p_des = spi_command.q_des_abad[0];
+// l1_control.a.v_des = spi_command.qd_des_abad[0];
+// l1_control.a.kp = spi_command.kp_abad[0];
+// l1_control.a.kd = spi_command.kd_abad[0];
+// l1_control.a.t_ff = spi_command.tau_abad_ff[0];
+
l1_control.h.p_des = spi_command.q_des_hip[0];
l1_control.h.v_des = spi_command.qd_des_hip[0];
l1_control.h.kp = spi_command.kp_hip[0];
l1_control.h.kd = spi_command.kd_hip[0];
l1_control.h.t_ff = spi_command.tau_hip_ff[0];
-
+
l1_control.k.p_des = spi_command.q_des_knee[0];
l1_control.k.v_des = spi_command.qd_des_knee[0];
l1_control.k.kp = spi_command.kp_knee[0];
l1_control.k.kd = spi_command.kd_knee[0];
l1_control.k.t_ff = spi_command.tau_knee_ff[0];
-
- l2_control.a.p_des = spi_command.q_des_abad[1];
- l2_control.a.v_des = spi_command.qd_des_abad[1];
- l2_control.a.kp = spi_command.kp_abad[1];
- l2_control.a.kd = spi_command.kd_abad[1];
- l2_control.a.t_ff = spi_command.tau_abad_ff[1];
-
+
+// l2_control.a.p_des = spi_command.q_des_abad[1];
+// l2_control.a.v_des = spi_command.qd_des_abad[1];
+// l2_control.a.kp = spi_command.kp_abad[1];
+// l2_control.a.kd = spi_command.kd_abad[1];
+// l2_control.a.t_ff = spi_command.tau_abad_ff[1];
+
l2_control.h.p_des = spi_command.q_des_hip[1];
l2_control.h.v_des = spi_command.qd_des_hip[1];
l2_control.h.kp = spi_command.kp_hip[1];
l2_control.h.kd = spi_command.kd_hip[1];
l2_control.h.t_ff = spi_command.tau_hip_ff[1];
-
+
l2_control.k.p_des = spi_command.q_des_knee[1];
l2_control.k.v_des = spi_command.qd_des_knee[1];
l2_control.k.kp = spi_command.kp_knee[1];
l2_control.k.kd = spi_command.kd_knee[1];
l2_control.k.t_ff = spi_command.tau_knee_ff[1];
-
-
+
+
spi_data.flags[0] = 0;
spi_data.flags[1] = 0;
- spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
- spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
+ //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
+ //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
//spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
- spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
- spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
+ //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
+ //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
//spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
//spi_data.flags[0] = 0xbeef;
@@ -524,7 +604,8 @@
// }
spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
for(int i = 0; i < DATA_LEN; i++){
- tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
+ tx_buff[i] = ((uint16_t*)(&spi_data))[i];
+ }
}
@@ -560,8 +641,7 @@
cs.fall(&spi_isr);
printf("done\n\r");
}
-
-
+
int main() {
wait(1);
//led = 1;
@@ -575,10 +655,10 @@
//can1.frequency(1000000); // set bit rate to 1Mbps
//can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
- can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+ can1.filter(0, 0x7F8, CANStandard, 0); //set up can filter
//can2.frequency(1000000); // set bit rate to 1Mbps
//can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
- can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+ can2.filter(0, 0x7F8, CANStandard, 0); //set up can filter
memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
memset(&spi_data, 0, sizeof(spi_data_t));
@@ -608,12 +688,12 @@
k1_can.id = 0x3;
k2_can.id = 0x3;
- pack_cmd(&a1_can, l1_control.a);
- pack_cmd(&a2_can, l2_control.a);
+// pack_cmd(&a1_can, l1_control.a);
+// pack_cmd(&a2_can, l2_control.a);
pack_cmd(&h1_can, l1_control.h);
pack_cmd(&h2_can, l2_control.h);
- pack_cmd(&k1_can, l1_control.k);
- pack_cmd(&k2_can, l2_control.k);
+ pack_cmd2(&k1_can, l1_control.k);
+ pack_cmd2(&k2_can, l2_control.k);
WriteAll();
@@ -633,6 +713,9 @@
unpack_reply(rxMsg1, &l1_state);
wait_us(10);
+ int32_t en_flag = spi_command.flags[0];
+ pc.printf("%1X", (en_flag&0xF));
+
// counter++;
// can1.read(rxMsg1);
// unpack_reply(rxMsg1, &l1_state);