Aditya Mehrotra
Updated for checksum
Diff: main.cpp
- Revision:
- 5:f1703165ca7e
- Parent:
- 4:e8c1b3f8fc6c
- Child:
- 6:077e37d5c2da
diff -r e8c1b3f8fc6c -r f1703165ca7e main.cpp
--- a/main.cpp Thu Apr 01 17:53:19 2021 +0000
+++ b/main.cpp Thu Apr 01 21:28:37 2021 +0000
@@ -29,9 +29,9 @@
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
- #define KD_MAX 5.0f
- #define T_MIN -18.0f
- #define T_MAX 18.0f
+ #define KD_MAX 10.0f
+ #define T_MIN -72.0f
+ #define T_MAX 72.0f
/// Joint Soft Stops ///
#define A_LIM_P 1.5f
@@ -57,13 +57,13 @@
Serial pc(PA_2, PA_3);
-CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name
-CAN can2(PA_11, PA_12, 1000000); // CAN Rx pin name, CAN Tx pin name
-CAN can3(PA_8, PA_15, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN4 on board
+CAN can1(PA_11, PA_12, 1000000);
+CAN can2(PA_8, PA_15, 1000000);
+CAN can3(PB_12, PB_13, 1000000); //corresponds to bus 1-3-6 (controller 1) or 2-4-5 (controller 2) IN THAT ORDER
CANMessage rxMsg1, rxMsg2, rxMsg3;
CANMessage txMsg1, txMsg2, txMsg3;
-CANMessage q11_can, q21_can, q31_can, q12_can, q22_can, q32_can, q13_can, q23_can, q33_can; //TX Messages
+CANMessage q11_can, q12_can, q13_can, q21_can, q22_can, q23_can, q31_can, q32_can, q33_can; //TX Messages
int ledState;
Ticker sendCAN;
int counter = 0;
@@ -77,7 +77,7 @@
grouped_act_state g1_state, g2_state, g3_state;
-grouped_act_control q1_control, q2_control, q3_control;
+grouped_act_control g1_control, g2_control, g3_control;
uint16_t x = 0;
uint16_t x2 = 0;
@@ -159,19 +159,19 @@
float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
if(id==1){
- group->a.p = p;
- group->a.v = v;
- group->a.t = t;
+ group->a1.p = p;
+ group->a1.v = v;
+ group->a1.t = t;
}
else if(id==2){
- group->h.p = p;
- group->h.v = v;
- group->h.t = t;
+ group->a2.p = p;
+ group->a2.v = v;
+ group->a2.t = t;
}
else if(id==3){
- group->k.p = p;
- group->k.v = v;
- group->k.t = t;
+ group->a3.p = p;
+ group->a3.v = v;
+ group->a3.t = t;
}
}
@@ -189,34 +189,39 @@
}
void PackAll(){
- pack_cmd(&q11_can, q1_control.a);
- pack_cmd(&q21_can, q1_control.h);
- pack_cmd(&q31_can, q1_control.k);
- pack_cmd(&q12_can, q2_control.a);
- pack_cmd(&q22_can, q2_control.h);
- pack_cmd(&q32_can, q2_control.k);
- pack_cmd(&q13_can, q3_control.a);
- pack_cmd(&q23_can, q3_control.h);
- pack_cmd(&q33_can, q3_control.k);
-
+ //actuators on the CAN1 bus
+ pack_cmd(&q11_can, q1_control.a1);
+ pack_cmd(&q12_can, q1_control.a2);
+ pack_cmd(&q13_can, q1_control.a3);
+ //actuators on the CAN2 bus
+ pack_cmd(&q21_can, q2_control.a1);
+ pack_cmd(&q22_can, q2_control.a2);
+ pack_cmd(&q23_can, q2_control.a3);
+ //actuators on the CAN3 bus
+ pack_cmd(&q31_can, q3_control.a1);
+ pack_cmd(&q32_can, q3_control.a2);
+ pack_cmd(&q33_can, q3_control.a3);
}
void WriteAll(){
//toggle = 1;
+ //ID = 1 actuators
can1.write(q11_can);
wait(.00002);
- can2.write(q12_can);
+ can2.write(q21_can);
wait(.00002);
- can3.write(q13_can);
+ can3.write(q31_can);
wait(.00002);
- can1.write(q21_can);
+ //ID = 2 actuators
+ can1.write(q12_can);
wait(.00002);
can2.write(q22_can);
wait(.00002);
- can3.write(q23_can);
+ can3.write(q32_can);
wait(.00002);
- can1.write(q31_can);
+ //ID = 3 actuators
+ can1.write(q13_can);
wait(.00002);
- can2.write(q32_can);
+ can2.write(q23_can);
wait(.00002);
can3.write(q33_can);
wait(.00002);
@@ -274,6 +279,7 @@
msg->data[7] = 0xFD;
WriteAll();
}
+
void serial_isr(){
/// handle keyboard commands from the serial terminal ///
while(pc.readable()){
@@ -283,29 +289,38 @@
case(27):
//loop.detach();
pc.printf("\n\r exiting motor mode \n\r");
+ //CAN BUS 1
ExitMotorMode(&q11_can);
+ ExitMotorMode(&q12_can);
+ ExitMotorMode(&q13_can);
+ //CAN BUS 2
ExitMotorMode(&q21_can);
- ExitMotorMode(&q31_can);
- ExitMotorMode(&q12_can);
ExitMotorMode(&q22_can);
+ ExitMotorMode(&q23_can);
+ //CAN BUS 3
+ ExitMotorMode(&q31_can);
ExitMotorMode(&q32_can);
- ExitMotorMode(&q13_can);
- ExitMotorMode(&q23_can);
ExitMotorMode(&q33_can);
+ //DISABLE FLAG
enabled = 0;
break;
case('m'):
pc.printf("\n\r entering motor mode \n\r");
+ //CAN BUS 1
EnterMotorMode(&q11_can);
+ EnterMotorMode(&q12_can);
+ EnterMotorMode(&q13_can);
+ //CAN BUS 2
EnterMotorMode(&q21_can);
- EnterMotorMode(&q31_can);
- EnterMotorMode(&q12_can);
EnterMotorMode(&q22_can);
+ EnterMotorMode(&q23_can);
+ //CAN BUS 3
+ EnterMotorMode(&q31_can);
EnterMotorMode(&q32_can);
- EnterMotorMode(&q13_can);
- EnterMotorMode(&q23_can);
EnterMotorMode(&q33_can);
+ //WAIT FOR ENABLE
wait(.5);
+ //ENABLE FLAG
enabled = 1;
//loop.attach(&sendCMD, .001);
break;
@@ -317,14 +332,17 @@
break;
case('z'):
pc.printf("\n\r zeroing \n\r");
+ //CAN BUS 1
Zero(&q11_can);
- Zero(&q21_can);
- Zero(&q31_can);
Zero(&q12_can);
+ Zero(&q13_can);
+ //CAN BUS 2
+ Zero(&q21_can);
Zero(&q22_can);
+ Zero(&q23_can);
+ //CAN BUS 3
+ Zero(&q31_can);
Zero(&q32_can);
- Zero(&q13_can);
- Zero(&q23_can);
Zero(&q33_can);
break;
}
@@ -345,60 +363,60 @@
void print_SPI_command() {
pc.printf("SPI MESSAGE RECIEVED:\n");
-
- pc.printf("MOTOR 1 Q: %f\n", spi_command.q_des_1s[0]);
- pc.printf("MOTOR 1 Qd: %f\n", spi_command.qd_des_1s[0]);
- pc.printf("MOTOR 1 Kp: %f\n", spi_command.kp_1s[0]);
- pc.printf("MOTOR 1 Kd: %f\n", spi_command.kd_1s[0]);
- pc.printf("MOTOR 1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
+ //CAN ONE
+ pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]);
+ pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]);
+ pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]);
+ pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]);
+ pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
- pc.printf("MOTOR 2 Q: %f\n", spi_command.q_des_2s[0]);
- pc.printf("MOTOR 2 Qd: %f\n", spi_command.qd_des_2s[0]);
- pc.printf("MOTOR 2 Kp: %f\n", spi_command.kp_2s[0]);
- pc.printf("MOTOR 2 Kd: %f\n", spi_command.kd_2s[0]);
- pc.printf("MOTOR 2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
+ pc.printf("MOTOR 1-2 Q: %f\n", spi_command.q_des_2s[0]);
+ pc.printf("MOTOR 1-2 Qd: %f\n", spi_command.qd_des_2s[0]);
+ pc.printf("MOTOR 1-2 Kp: %f\n", spi_command.kp_2s[0]);
+ pc.printf("MOTOR 1-2 Kd: %f\n", spi_command.kd_2s[0]);
+ pc.printf("MOTOR 1-2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
- pc.printf("MOTOR 3 Q: %f\n", spi_command.q_des_3s[0]);
- pc.printf("MOTOR 3 Qd: %f\n", spi_command.qd_des_3s[0]);
- pc.printf("MOTOR 3 Kp: %f\n", spi_command.kp_3s[0]);
- pc.printf("MOTOR 3 Kd: %f\n", spi_command.kd_3s[0]);
- pc.printf("MOTOR 3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
+ pc.printf("MOTOR 1-3 Q: %f\n", spi_command.q_des_3s[0]);
+ pc.printf("MOTOR 1-3 Qd: %f\n", spi_command.qd_des_3s[0]);
+ pc.printf("MOTOR 1-3 Kp: %f\n", spi_command.kp_3s[0]);
+ pc.printf("MOTOR 1-3 Kd: %f\n", spi_command.kd_3s[0]);
+ pc.printf("MOTOR 1-3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
- pc.printf("MOTOR 4 Q: %f\n", spi_command.q_des_1s[1]);
- pc.printf("MOTOR 4 Qd: %f\n", spi_command.qd_des_1s[1]);
- pc.printf("MOTOR 4 Kp: %f\n", spi_command.kp_1s[1]);
- pc.printf("MOTOR 4 Kd: %f\n", spi_command.kd_1s[1]);
- pc.printf("MOTOR 4 T_FF: %f\n", spi_command.tau_1s_ff[1]);
+ pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]);
+ pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]);
+ pc.printf("MOTOR 2-1 Kp: %f\n", spi_command.kp_1s[1]);
+ pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]);
+ pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]);
- pc.printf("MOTOR 5 Q: %f\n", spi_command.q_des_2s[1]);
- pc.printf("MOTOR 5 Qd: %f\n", spi_command.qd_des_2s[1]);
- pc.printf("MOTOR 5 Kp: %f\n", spi_command.kp_2s[1]);
- pc.printf("MOTOR 5 Kd: %f\n", spi_command.kd_2s[1]);
- pc.printf("MOTOR 5 T_FF: %f\n", spi_command.tau_2s_ff[1]);
+ pc.printf("MOTOR 2-2 Q: %f\n", spi_command.q_des_2s[1]);
+ pc.printf("MOTOR 2-2 Qd: %f\n", spi_command.qd_des_2s[1]);
+ pc.printf("MOTOR 2-2 Kp: %f\n", spi_command.kp_2s[1]);
+ pc.printf("MOTOR 2-2 Kd: %f\n", spi_command.kd_2s[1]);
+ pc.printf("MOTOR 2-2 T_FF: %f\n", spi_command.tau_2s_ff[1]);
- pc.printf("MOTOR 6 Q: %f\n", spi_command.q_des_3s[1]);
- pc.printf("MOTOR 6 Qd: %f\n", spi_command.qd_des_3s[1]);
- pc.printf("MOTOR 6 Kp: %f\n", spi_command.kp_3s[1]);
- pc.printf("MOTOR 6 Kd: %f\n", spi_command.kd_3s[1]);
- pc.printf("MOTOR 6 T_FF: %f\n", spi_command.tau_3s_ff[1]);
+ pc.printf("MOTOR 2-3 Q: %f\n", spi_command.q_des_3s[1]);
+ pc.printf("MOTOR 2-3 Qd: %f\n", spi_command.qd_des_3s[1]);
+ pc.printf("MOTOR 2-3 Kp: %f\n", spi_command.kp_3s[1]);
+ pc.printf("MOTOR 2-3 Kd: %f\n", spi_command.kd_3s[1]);
+ pc.printf("MOTOR 2-3 T_FF: %f\n", spi_command.tau_3s_ff[1]);
- pc.printf("MOTOR 7 Q: %f\n", spi_command.q_des_1s[2]);
- pc.printf("MOTOR 7 Qd: %f\n", spi_command.qd_des_1s[2]);
- pc.printf("MOTOR 7 Kp: %f\n", spi_command.kp_1s[2]);
- pc.printf("MOTOR 7 Kd: %f\n", spi_command.kd_1s[2]);
- pc.printf("MOTOR 7 T_FF: %f\n", spi_command.tau_1s_ff[2]);
+ pc.printf("MOTOR 3-1 Q: %f\n", spi_command.q_des_1s[2]);
+ pc.printf("MOTOR 3-1 Qd: %f\n", spi_command.qd_des_1s[2]);
+ pc.printf("MOTOR 3-1 Kp: %f\n", spi_command.kp_1s[2]);
+ pc.printf("MOTOR 3-1 Kd: %f\n", spi_command.kd_1s[2]);
+ pc.printf("MOTOR 3-1 T_FF: %f\n", spi_command.tau_1s_ff[2]);
- pc.printf("MOTOR 8 Q: %f\n", spi_command.q_des_2s[2]);
- pc.printf("MOTOR 8 Qd: %f\n", spi_command.qd_des_2s[2]);
- pc.printf("MOTOR 8 Kp: %f\n", spi_command.kp_2s[2]);
- pc.printf("MOTOR 8 Kd: %f\n", spi_command.kd_2s[2]);
- pc.printf("MOTOR 8 T_FF: %f\n", spi_command.tau_2s_ff[2]);
+ pc.printf("MOTOR 3-2 Q: %f\n", spi_command.q_des_2s[2]);
+ pc.printf("MOTOR 3-2 Qd: %f\n", spi_command.qd_des_2s[2]);
+ pc.printf("MOTOR 3-2 Kp: %f\n", spi_command.kp_2s[2]);
+ pc.printf("MOTOR 3-2 Kd: %f\n", spi_command.kd_2s[2]);
+ pc.printf("MOTOR 3-2 T_FF: %f\n", spi_command.tau_2s_ff[2]);
- pc.printf("MOTOR 9 Q: %f\n", spi_command.q_des_3s[2]);
- pc.printf("MOTOR 9 Qd: %f\n", spi_command.qd_des_3s[2]);
- pc.printf("MOTOR 9 Kp: %f\n", spi_command.kp_3s[2]);
- pc.printf("MOTOR 9 Kd: %f\n", spi_command.kd_3s[2]);
- pc.printf("MOTOR 9 T_FF: %f\n", spi_command.tau_3s_ff[2]);
+ pc.printf("MOTOR 3-3 Q: %f\n", spi_command.q_des_3s[2]);
+ pc.printf("MOTOR 3-3 Qd: %f\n", spi_command.qd_des_3s[2]);
+ pc.printf("MOTOR 3-3 Kp: %f\n", spi_command.kp_3s[2]);
+ pc.printf("MOTOR 3-3 Kd: %f\n", spi_command.kd_3s[2]);
+ pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]);
}
@@ -433,7 +451,7 @@
((uint16_t*)(&spi_command))[i] = rx_buff[i];
//pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]);
}
- //print_SPI_command();
+ print_SPI_command();
// run control, which fills in tx_buff for the next iteration
/*
@@ -443,9 +461,7 @@
pc.printf("ACTUAL: %d\n", calc_checksum);
pc.printf("CURRENT: %d\n", spi_command.checksum);}
*/
-
- //test_control();
- //spi_data.q_abad[0] = 12.0f;
+
control();
PackAll();
WriteAll();
@@ -490,24 +506,25 @@
//BUS ONE
EnterMotorMode(&q11_can);
can1.write(q11_can);
+ EnterMotorMode(&q12_can);
+ can1.write(q12_can);
+ EnterMotorMode(&q13_can);
+ can1.write(q13_can);
+ //BUS TWO
EnterMotorMode(&q21_can);
- can1.write(q21_can);
- EnterMotorMode(&q31_can);
- can1.write(q31_can);
- //BUS TWO
- EnterMotorMode(&q12_can);
- can2.write(q12_can);
+ can2.write(q21_can);
EnterMotorMode(&q22_can);
can2.write(q22_can);
- EnterMotorMode(&q32_can);
- can2.write(q32_can);
+ EnterMotorMode(&q23_can);
+ can2.write(q23_can);
//BUS THREE
- EnterMotorMode(&q13_can);
- can3.write(q13_can);
- EnterMotorMode(&q23_can);
- can3.write(q23_can);
+ EnterMotorMode(&q31_can);
+ can3.write(q31_can);
+ EnterMotorMode(&q32_can);
+ can3.write(q32_can);
EnterMotorMode(&q33_can);
can3.write(q33_can);
+ //SERIAL TO USER
pc.printf("e\n\r");
return;
}
@@ -516,53 +533,53 @@
//BUS ONE
ExitMotorMode(&q11_can);
can1.write(q11_can);
+ ExitMotorMode(&q12_can);
+ can1.write(q12_can);
+ ExitMotorMode(&q13_can);
+ can1.write(q13_can);
+ //BUS TWO
ExitMotorMode(&q21_can);
- can1.write(q21_can);
- ExitMotorMode(&q31_can);
- can1.write(q31_can);
- //BUS TWO
- ExitMotorMode(&q12_can);
- can2.write(q12_can);
+ can2.write(q21_can);
ExitMotorMode(&q22_can);
can2.write(q22_can);
- ExitMotorMode(&q32_can);
- can2.write(q32_can);
+ ExitMotorMode(&q23_can);
+ can2.write(q23_can);
//BUS THREE
- ExitMotorMode(&q13_can);
- can3.write(q13_can);
- ExitMotorMode(&q23_can);
- can3.write(q23_can);
+ ExitMotorMode(&q31_can);
+ can3.write(q31_can);
+ ExitMotorMode(&q32_can);
+ can3.write(q32_can);
ExitMotorMode(&q33_can);
can3.write(q33_can);
+ //SERIAL TO USER
pc.printf("x\n\r");
return;
}
- spi_data.q_1s[0] = g1_state.a.p;
- spi_data.q_2s[0] = g1_state.h.p;
- spi_data.q_3s[0] = g1_state.k.p;
- spi_data.qd_1s[0] = g1_state.a.v;
- spi_data.qd_2s[0] = g1_state.h.v;
- spi_data.qd_3s[0] = g1_state.k.v;
-
- spi_data.q_1s[1] = g1_state.a.p;
- spi_data.q_2s[1] = g1_state.h.p;
- spi_data.q_3s[1] = g1_state.k.p;
- spi_data.qd_1s[1] = g1_state.a.v;
- spi_data.qd_2s[1] = g1_state.h.v;
- spi_data.qd_3s[1] = g1_state.k.v;
-
- spi_data.q_1s[2] = g1_state.a.p;
- spi_data.q_2s[2] = g1_state.h.p;
- spi_data.q_3s[2] = g1_state.k.p;
- spi_data.qd_1s[2] = g1_state.a.v;
- spi_data.qd_2s[2] = g1_state.h.v;
- spi_data.qd_3s[2] = g1_state.k.v;
-
-
+ //BUS 1 DATA
+ spi_data.q_1s[0] = g1_state.a1.p;
+ spi_data.q_2s[0] = g1_state.a2.p;
+ spi_data.q_3s[0] = g1_state.a3.p;
+ spi_data.qd_1s[0] = g1_state.a1.v;
+ spi_data.qd_2s[0] = g1_state.a2.v;
+ spi_data.qd_3s[0] = g1_state.a3.v;
+ //BUS 2 DATA
+ spi_data.q_1s[1] = g1_state.a1.p;
+ spi_data.q_2s[1] = g1_state.a2.p;
+ spi_data.q_3s[1] = g1_state.a3.p;
+ spi_data.qd_1s[1] = g1_state.a1.v;
+ spi_data.qd_2s[1] = g1_state.a2.v;
+ spi_data.qd_3s[1] = g1_state.a3.v;
+ //BUS 3 DATA
+ spi_data.q_1s[2] = g1_state.a1.p;
+ spi_data.q_2s[2] = g1_state.a2.p;
+ spi_data.q_3s[2] = g1_state.a3.p;
+ spi_data.qd_1s[2] = g1_state.a1.v;
+ spi_data.qd_2s[2] = g1_state.a2.v;
+ spi_data.qd_3s[2] = g1_state.a3.v;
if(estop==0){
- //printf("estopped!!!!\n\r");
+ printf("estopped!!!!\n\r");
memset(&q1_control, 0, sizeof(q1_control));
memset(&q2_control, 0, sizeof(q2_control));
memset(&q3_control, 0, sizeof(q3_control));
@@ -574,78 +591,81 @@
else{
led = 0;
-
memset(&q1_control, 0, sizeof(q1_control));
memset(&q2_control, 0, sizeof(q2_control));
memset(&q3_control, 0, sizeof(q3_control));
-
- q1_control.a.p_des = spi_command.q_des_1s[0];
- q1_control.a.v_des = spi_command.qd_des_1s[0];
- q1_control.a.kp = spi_command.kp_1s[0];
- q1_control.a.kd = spi_command.kd_1s[0];
- q1_control.a.t_ff = spi_command.tau_1s_ff[0];
-
- q1_control.h.p_des = spi_command.q_des_2s[0];
- q1_control.h.v_des = spi_command.qd_des_2s[0];
- q1_control.h.kp = spi_command.kp_2s[0];
- q1_control.h.kd = spi_command.kd_2s[0];
- q1_control.h.t_ff = spi_command.tau_2s_ff[0];
-
- q1_control.k.p_des = spi_command.q_des_3s[0];
- q1_control.k.v_des = spi_command.qd_des_3s[0];
- q1_control.k.kp = spi_command.kp_3s[0];
- q1_control.k.kd = spi_command.kd_3s[0];
- q1_control.k.t_ff = spi_command.tau_3s_ff[0];
-
- q2_control.a.p_des = spi_command.q_des_1s[1];
- q2_control.a.v_des = spi_command.qd_des_1s[1];
- q2_control.a.kp = spi_command.kp_1s[1];
- q2_control.a.kd = spi_command.kd_1s[1];
- q2_control.a.t_ff = spi_command.tau_1s_ff[1];
+ //TRANSLATE SPI TO ACTUATOR COMMANNDS
+ //CAN1
+ //CAN1 MOTOR1
+ g1_control.a1.p_des = spi_command.q_des_1s[0];
+ g1_control.a1.v_des = spi_command.qd_des_1s[0];
+ g1_control.a1.kp = spi_command.kp_1s[0];
+ g1_control.a1.kd = spi_command.kd_1s[0];
+ g1_control.a1.t_ff = spi_command.tau_1s_ff[0];
+ //CAN1 MOTOR 2
+ g1_control.a2.p_des = spi_command.q_des_2s[0];
+ g1_control.a2.v_des = spi_command.qd_des_2s[0];
+ g1_control.a2.kp = spi_command.kp_2s[0];
+ g1_control.a2.kd = spi_command.kd_2s[0];
+ g1_control.a2.t_ff = spi_command.tau_2s_ff[0];
+ //CAN1 MOTOR 3
+ g1_control.a3.p_des = spi_command.q_des_3s[0];
+ g1_control.a3.v_des = spi_command.qd_des_3s[0];
+ g1_control.a3.kp = spi_command.kp_3s[0];
+ g1_control.a3.kd = spi_command.kd_3s[0];
+ g1_control.a3.t_ff = spi_command.tau_3s_ff[0];
+ //CAN2
+ //CAN2 MOTOR1
+ g2_control.a1.p_des = spi_command.q_des_1s[1];
+ g2_control.a1.v_des = spi_command.qd_des_1s[1];
+ g2_control.a1.kp = spi_command.kp_1s[1];
+ g2_control.a1.kd = spi_command.kd_1s[1];
+ g2_control.a1.t_ff = spi_command.tau_1s_ff[1];
+ //CAN2 MOTOR 2
+ g2_control.a2.p_des = spi_command.q_des_2s[1];
+ g2_control.a2.v_des = spi_command.qd_des_2s[1];
+ g2_control.a2.kp = spi_command.kp_2s[1];
+ g2_control.a2.kd = spi_command.kd_2s[1];
+ g2_control.a2.t_ff = spi_command.tau_2s_ff[1];
+ //CAN2 MOTOR 3
+ g2_control.a3.p_des = spi_command.q_des_3s[1];
+ g2_control.a3.v_des = spi_command.qd_des_3s[1];
+ g2_control.a3.kp = spi_command.kp_3s[1];
+ g2_control.a3.kd = spi_command.kd_3s[1];
+ g2_control.a3.t_ff = spi_command.tau_3s_ff[1];
+ //CAN3
+ //CAN3 MOTOR1
+ g3_control.a1.p_des = spi_command.q_des_1s[2];
+ g3_control.a1.v_des = spi_command.qd_des_1s[2];
+ g3_control.a1.kp = spi_command.kp_1s[2];
+ g3_control.a1.kd = spi_command.kd_1s[2];
+ g3_control.a1.t_ff = spi_command.tau_1s_ff[2];
+ //CAN3 MOTOR 2
+ g3_control.a2.p_des = spi_command.q_des_2s[2];
+ g3_control.a2.v_des = spi_command.qd_des_2s[2];
+ g3_control.a2.kp = spi_command.kp_2s[2];
+ g3_control.a2.kd = spi_command.kd_2s[2];
+ g3_control.a2.t_ff = spi_command.tau_2s_ff[2];
+ //CAN3 MOTOR 3
+ g3_control.a3.p_des = spi_command.q_des_3s[2];
+ g3_control.a3.v_des = spi_command.qd_des_3s[2];
+ g3_control.a3.kp = spi_command.kp_3s[2];
+ g3_control.a3.kd = spi_command.kd_3s[2];
+ g3_control.a3.t_ff = spi_command.tau_3s_ff[2];
- q2_control.h.p_des = spi_command.q_des_2s[1];
- q2_control.h.v_des = spi_command.qd_des_2s[1];
- q2_control.h.kp = spi_command.kp_2s[1];
- q2_control.h.kd = spi_command.kd_2s[1];
- q2_control.h.t_ff = spi_command.tau_2s_ff[1];
-
- q2_control.k.p_des = spi_command.q_des_3s[1];
- q2_control.k.v_des = spi_command.qd_des_3s[1];
- q2_control.k.kp = spi_command.kp_3s[1];
- q2_control.k.kd = spi_command.kd_3s[1];
- q2_control.k.t_ff = spi_command.tau_3s_ff[1];
-
- q3_control.a.p_des = spi_command.q_des_1s[2];
- q3_control.a.v_des = spi_command.qd_des_1s[2];
- q3_control.a.kp = spi_command.kp_1s[2];
- q3_control.a.kd = spi_command.kd_1s[2];
- q3_control.a.t_ff = spi_command.tau_1s_ff[2];
-
- q3_control.h.p_des = spi_command.q_des_2s[2];
- q3_control.h.v_des = spi_command.qd_des_2s[2];
- q3_control.h.kp = spi_command.kp_2s[2];
- q3_control.h.kd = spi_command.kd_2s[2];
- q3_control.h.t_ff = spi_command.tau_2s_ff[2];
-
- q3_control.k.p_des = spi_command.q_des_3s[2];
- q3_control.k.v_des = spi_command.qd_des_3s[2];
- q3_control.k.kp = spi_command.kp_3s[2];
- q3_control.k.kd = spi_command.kd_3s[2];
- q3_control.k.t_ff = spi_command.tau_3s_ff[2];
-
-
+ //SPI FLAGS RETURN //MIGHT NEED FIXING LATER WHEN WE DO RETURN DATA
spi_data.flags[0] = 0;
spi_data.flags[1] = 0;
spi_data.flags[2] = 0;
- spi_data.flags[0] |= softstop_joint(g1_state.a, &q1_control.a, A_LIM_P, A_LIM_N);
- spi_data.flags[0] |= (softstop_joint(g1_state.h, &q1_control.h, H_LIM_P, H_LIM_N))<<1;
+ spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A_LIM_P, A_LIM_N);
+ spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, H_LIM_P, H_LIM_N))<<1;
//spi_data.flags[0] |= (softstop_joint(g1_state.k, &q1_control.k, K_LIM_P, K_LIM_N))<<2;
- spi_data.flags[1] |= softstop_joint(g2_state.a, &q2_control.a, A_LIM_P, A_LIM_N);
- spi_data.flags[1] |= (softstop_joint(g2_state.h, &q2_control.h, H_LIM_P, H_LIM_N))<<1;
+ spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A_LIM_P, A_LIM_N);
+ spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, H_LIM_P, H_LIM_N))<<1;
//spi_data.flags[1] |= (softstop_joint(g2_state.k, &q2_control.k, K_LIM_P, K_LIM_N))<<2;
- spi_data.flags[2] |= softstop_joint(g3_state.a, &q3_control.a, A_LIM_P, A_LIM_N);
- spi_data.flags[2] |= (softstop_joint(g3_state.h, &q3_control.h, H_LIM_P, H_LIM_N))<<1;
+ spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A_LIM_P, A_LIM_N);
+ spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, H_LIM_P, H_LIM_N))<<1;
//spi_data.flags[2] |= (softstop_joint(g3_state.k, &q3_control.k, K_LIM_P, K_LIM_N))<<2;
//spi_data.flags[0] = 0xbeef;
@@ -686,7 +706,7 @@
void init_spi(void){
SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
spi->format(16, 0);
- spi->frequency(12000000);
+ spi->frequency(6000000);
spi->reply(0x0);
cs.fall(&spi_isr);
pc.printf("done\n\r");
@@ -723,38 +743,49 @@
pc.printf("\n\r SPIne\n\r");
//printf("%d\n\r", RX_ID << 18);
+ //CAN 1 BUS
q11_can.len = 8; //transmit 8 bytes
+ q12_can.len = 8; //transmit 8 bytes
+ q13_can.len = 8;
+ //CAN 2 BUS
q21_can.len = 8; //transmit 8 bytes
- q31_can.len = 8;
- q12_can.len = 8; //transmit 8 bytes
q22_can.len = 8; //transmit 8 bytes
- q32_can.len = 8;
- q13_can.len = 8; //transmit 8 bytes
- q23_can.len = 8; //transmit 8 bytes
+ q23_can.len = 8;
+ //CAN 3 BUS
+ q31_can.len = 8; //transmit 8 bytes
+ q32_can.len = 8; //transmit 8 bytes
q33_can.len = 8;
+ //RECIEVE
rxMsg1.len = 6; //receive 6 bytes
rxMsg2.len = 6;
rxMsg3.len = 6;
-
+
+ //CAN 1 BUS
q11_can.id = 0x1;
- q21_can.id = 0x2;
- q31_can.id = 0x3;
- q12_can.id = 0x1;
+ q12_can.id = 0x2;
+ q13_can.id = 0x3;
+ //CAN 2 BUS
+ q21_can.id = 0x1;
q22_can.id = 0x2;
- q32_can.id = 0x3;
- q13_can.id = 0x1;
- q23_can.id = 0x2;
- q33_can.id = 0x3;
+ q23_can.id = 0x3;
+ //CAN 3 BUS
+ q31_can.id = 0x1;
+ q32_can.id = 0x2;
+ q33_can.id = 0x3;
- pack_cmd(&q11_can, q1_control.a);
- pack_cmd(&q12_can, q2_control.a);
- pack_cmd(&q13_can, q3_control.a);
- pack_cmd(&q21_can, q1_control.h);
- pack_cmd(&q22_can, q2_control.h);
- pack_cmd(&q23_can, q3_control.h);
- pack_cmd(&q31_can, q1_control.k);
- pack_cmd(&q32_can, q2_control.k);
- pack_cmd(&q33_can, q3_control.k);
+ //actuators on the CAN1 bus
+ pack_cmd(&q11_can, q1_control.a1);
+ pack_cmd(&q12_can, q1_control.a2);
+ pack_cmd(&q13_can, q1_control.a3);
+ //actuators on the CAN2 bus
+ pack_cmd(&q21_can, q2_control.a1);
+ pack_cmd(&q22_can, q2_control.a2);
+ pack_cmd(&q23_can, q2_control.a3);
+ //actuators on the CAN3 bus
+ pack_cmd(&q31_can, q3_control.a1);
+ pack_cmd(&q32_can, q3_control.a2);
+ pack_cmd(&q33_can, q3_control.a3);
+ //WRITE THE INITIAL COMMAND
WriteAll();
//just debugging things