Aditya Mehrotra
Updated for checksum
Revision 15:56a06363a698, committed 2022-09-28
- Comitter:
- adimmit
- Date:
- Wed Sep 28 16:32:46 2022 +0000
- Parent:
- 14:6fec697f62ef
- Commit message:
- updated to work with checksum and with the 2-motor dyno;
Changed in this revision
| leg_message.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/leg_message.h Fri Feb 04 22:38:23 2022 +0000
+++ b/leg_message.h Wed Sep 28 16:32:46 2022 +0000
@@ -3,43 +3,27 @@
#include <stdint.h>
-// 31*4 bytes = 124 8-bit words
-// 31 32-bit numbers
+// 9*2 bytes = 18 16-bit words 36 8-bit words
+// 9 32-bit numbers
struct spi_data_t
{
- float q_1s[3];
- float q_2s[3];
- float q_3s[3];
- float qd_1s[3];
- float qd_2s[3];
- float qd_3s[3];
- float tau_1s[3];
- float tau_2s[3];
- float tau_3s[3];
- uint32_t flags[3];
+ float q_1s[2];
+ float qd_1s[2];
+ float tau_1s[2];
+ int32_t flags[2];
uint32_t checksum;
};
-// 49*4 bytes = 196 8-bit words
-// 49 32-bit numbers
+// 13*2 bytes = 26 16-bit words 52 8-bit words
+// 13 32-bit numbers
struct spi_command_t
{
- float q_des_1s[3]; //first motor on each can bus 0-can1, 1-can2, 2-can3
- float q_des_2s[3];
- float q_des_3s[3];
- float qd_des_1s[3];
- float qd_des_2s[3];
- float qd_des_3s[3];
- float kp_1s[3];
- float kp_2s[3];
- float kp_3s[3];
- float kd_1s[3];
- float kd_2s[3];
- float kd_3s[3];
- float tau_1s_ff[3];
- float tau_2s_ff[3];
- float tau_3s_ff[3];
- uint32_t flags[3];
+ float q_des_1s[2]; //first motor on each can bus 0-can1, 1-can2, 2-can3
+ float qd_des_1s[2];
+ float kp_1s[2];
+ float kd_1s[2];
+ float tau_1s_ff[2];
+ int32_t flags[2];
uint32_t checksum;
};
@@ -54,10 +38,10 @@
};
struct grouped_act_state{
- joint_state a1, a2, a3; //removed a2 and a3
+ joint_state a1; //removed a2 and a3
};
struct grouped_act_control{
- joint_control a1, a2, a3;
+ joint_control a1;
}
;
#endif
\ No newline at end of file
--- a/main.cpp Fri Feb 04 22:38:23 2022 +0000
+++ b/main.cpp Wed Sep 28 16:32:46 2022 +0000
@@ -10,12 +10,12 @@
#include "leg_message.h"
// length of receive/transmit buffers
-#define RX_LEN 98 //CHECK THESE BUFFER LENGHTS
-#define TX_LEN 62 //CHECK THESE BUFFER LENGHTS
+#define RX_LEN 52 //CHECK THESE BUFFER LENGHTS
+#define TX_LEN 52 //CHECK THESE BUFFER LENGHTS
// length of outgoing/incoming messages
-#define DATA_LEN 62 //CHECK THESE BUFFER LENGHTS
-#define CMD_LEN 98 //CHECK THESE BUFFER LENGHTS
+#define DATA_LEN 36 //CHECK THESE BUFFER LENGHTS
+#define CMD_LEN 52 //CHECK THESE BUFFER LENGHTS
// Master CAN ID ///
#define CAN_ID 0x0
@@ -64,7 +64,7 @@
CANMessage rxMsg1, rxMsg2, rxMsg3;
CANMessage txMsg1, txMsg2, txMsg3;
-CANMessage q11_can, q12_can, q13_can, q21_can, q22_can, q23_can, q31_can, q32_can, q33_can; //TX Messages
+CANMessage q11_can, q21_can; //q12_can, q13_can, q22_can, q23_can, q31_can, q32_can, q33_can; //TX Messages
int ledState;
Ticker sendCAN;
int counter = 0;
@@ -77,8 +77,8 @@
//SPISlave spi(PA_7, PA_6, PA_5, PA_4);
-grouped_act_state g1_state, g2_state, g3_state;
-grouped_act_control g1_control, g2_control, g3_control;
+grouped_act_state g1_state, g2_state; // g3_state;
+grouped_act_control g1_control, g2_control; // g3_control;
uint16_t x = 0;
uint16_t x2 = 0;
@@ -164,16 +164,6 @@
group->a1.v = v;
group->a1.t = t;
}
- else if(id==2){
- group->a2.p = p;
- group->a2.v = v;
- group->a2.t = t;
- }
- else if(id==3){
- group->a3.p = p;
- group->a3.v = v;
- group->a3.t = t;
- }
}
void rxISR1() {
@@ -184,24 +174,12 @@
can2.read(rxMsg2);
unpack_reply(rxMsg2, &g2_state);
}
-void rxISR3(){
- can3.read(rxMsg3);
- unpack_reply(rxMsg3, &g3_state);
- }
void PackAll(){
//actuators on the CAN1 bus
pack_cmd(&q11_can, g1_control.a1);
- pack_cmd(&q12_can, g1_control.a2);
- pack_cmd(&q13_can, g1_control.a3);
//actuators on the CAN2 bus
pack_cmd(&q21_can, g2_control.a1);
- pack_cmd(&q22_can, g2_control.a2);
- pack_cmd(&q23_can, g2_control.a3);
- //actuators on the CAN3 bus
- pack_cmd(&q31_can, g3_control.a1);
- pack_cmd(&q32_can, g3_control.a2);
- pack_cmd(&q33_can, g3_control.a3);
}
void WriteAll(){
//toggle = 1;
@@ -210,23 +188,6 @@
wait(.00002);
can2.write(q21_can);
wait(.00002);
- can3.write(q31_can);
- wait(.00002);
- //ID = 2 actuators
- can1.write(q12_can);
- wait(.00002);
- can2.write(q22_can);
- wait(.00002);
- can3.write(q32_can);
- wait(.00002);
- //ID = 3 actuators
- can1.write(q13_can);
- wait(.00002);
- can2.write(q23_can);
- wait(.00002);
- can3.write(q33_can);
- wait(.00002);
- //toggle = 0;
}
void sendCMD(){
@@ -235,7 +196,7 @@
PackAll();
if(counter>100){
- pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", g1_state.a1.p, g1_state.a2.p, g1_state.a3.p, g2_state.a1.p, g2_state.a2.p, g2_state.a3.p, g3_state.a1.p, g3_state.a2.p, g3_state.a3.p);
+ pc.printf("%.3f %.3f\n\r", g1_state.a1.p, g2_state.a1.p);
counter = 0 ;
}
@@ -293,16 +254,8 @@
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(&q22_can);
- ExitMotorMode(&q23_can);
- //CAN BUS 3
- ExitMotorMode(&q31_can);
- ExitMotorMode(&q32_can);
- ExitMotorMode(&q33_can);
//DISABLE FLAG
enabled = 0;
break;
@@ -310,16 +263,8 @@
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(&q22_can);
- EnterMotorMode(&q23_can);
- //CAN BUS 3
- EnterMotorMode(&q31_can);
- EnterMotorMode(&q32_can);
- EnterMotorMode(&q33_can);
//WAIT FOR ENABLE
wait(.5);
//ENABLE FLAG
@@ -336,16 +281,16 @@
pc.printf("\n\r zeroing \n\r");
//CAN BUS 1
Zero(&q11_can);
- Zero(&q12_can);
- Zero(&q13_can);
+ //Zero(&q12_can);
+ //Zero(&q13_can);
//CAN BUS 2
Zero(&q21_can);
- Zero(&q22_can);
- Zero(&q23_can);
+ //Zero(&q22_can);
+ //Zero(&q23_can);
//CAN BUS 3
- Zero(&q31_can);
- Zero(&q32_can);
- Zero(&q33_can);
+ //Zero(&q31_can);
+ //Zero(&q32_can);
+ //Zero(&q33_can);
break;
}
}
@@ -366,61 +311,19 @@
void print_SPI_command() {
pc.printf("SPI MESSAGE RECIEVED:\n");
- //CAN ONE
+ //CAN ID 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 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 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 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 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 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 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 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 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]);
-
}
@@ -428,34 +331,13 @@
void print_SPI_data() {
pc.printf("SPI MESSAGE SENT:\n");
- //CAN ONE
+ //CAN ID ONES
pc.printf("MOTOR 1-1 Q: %f\n", spi_data.q_1s[0]);
pc.printf("MOTOR 1-1 Qd: %f\n", spi_data.qd_1s[0]);
- pc.printf("MOTOR 1-2 Q: %f\n", spi_data.q_2s[0]);
- pc.printf("MOTOR 1-2 Qd: %f\n", spi_data.qd_2s[0]);
-
- pc.printf("MOTOR 1-3 Q: %f\n", spi_data.q_3s[0]);
- pc.printf("MOTOR 1-3 Qd: %f\n", spi_data.qd_3s[0]);
-
pc.printf("MOTOR 2-1 Q: %f\n", spi_data.q_1s[1]);
pc.printf("MOTOR 2-1 Qd: %f\n", spi_data.qd_1s[1]);
- pc.printf("MOTOR 2-2 Q: %f\n", spi_data.q_2s[1]);
- pc.printf("MOTOR 2-2 Qd: %f\n", spi_data.qd_2s[1]);
-
- pc.printf("MOTOR 2-3 Q: %f\n", spi_data.q_3s[1]);
- pc.printf("MOTOR 2-3 Qd: %f\n", spi_data.qd_3s[1]);
-
- pc.printf("MOTOR 3-1 Q: %f\n", spi_data.q_1s[2]);
- pc.printf("MOTOR 3-1 Qd: %f\n", spi_data.qd_1s[2]);
-
- pc.printf("MOTOR 3-2 Q: %f\n", spi_data.q_2s[2]);
- pc.printf("MOTOR 3-2 Qd: %f\n", spi_data.qd_2s[2]);
-
- pc.printf("MOTOR 3-3 Q: %f\n", spi_data.q_3s[2]);
- pc.printf("MOTOR 3-3 Qd: %f\n", spi_data.qd_3s[2]);
-
}
@@ -465,40 +347,18 @@
//pc.printf("%f\n", spi_command.q_des_2s[0]);
// update qs
spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0;
- spi_data.q_2s[0] = spi_command.q_des_2s[0]+1.0;
- spi_data.q_3s[0] = spi_command.q_des_3s[0]+1.0;
spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0;
- spi_data.q_2s[1] = spi_command.q_des_2s[1]+1.0;
- spi_data.q_3s[1] = spi_command.q_des_3s[1]+1.0;
- spi_data.q_1s[2] = spi_command.q_des_1s[2]+1.0;
- spi_data.q_2s[2] = spi_command.q_des_2s[2]+1.0;
- spi_data.q_3s[2] = spi_command.q_des_3s[2]+1.0;
// update qds
spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0;
- spi_data.qd_2s[0] = spi_command.qd_des_2s[0]+1.0;
- spi_data.qd_3s[0] = spi_command.qd_des_3s[0]+1.0;
spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0;
- spi_data.qd_2s[1] = spi_command.qd_des_2s[1]+1.0;
- spi_data.qd_3s[1] = spi_command.qd_des_3s[1]+1.0;
- spi_data.qd_1s[2] = spi_command.qd_des_1s[2]+1.0;
- spi_data.qd_2s[2] = spi_command.qd_des_2s[2]+1.0;
- spi_data.qd_3s[2] = spi_command.qd_des_3s[2]+1.0;
// update taus
spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0;
- spi_data.tau_2s[0] = spi_command.tau_2s_ff[0]+1.0;
- spi_data.tau_3s[0] = spi_command.tau_3s_ff[0]+1.0;
spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0;
- spi_data.tau_2s[1] = spi_command.tau_2s_ff[1]+1.0;
- spi_data.tau_3s[1] = spi_command.tau_3s_ff[1]+1.0;
- spi_data.tau_1s[2] = spi_command.tau_1s_ff[2]+1.0;
- spi_data.tau_2s[2] = spi_command.tau_2s_ff[2]+1.0;
- spi_data.tau_3s[2] = spi_command.tau_3s_ff[2]+1.0;
// UDPATE FLAGS
spi_data.flags[0] = 0;
spi_data.flags[1] = 0;
- spi_data.flags[2] = 0;
// UPDATE CHECKSUM
- spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30);
+ spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8); //DONT CRC THE CRC!
for(int i = 0; i < DATA_LEN; i++){
tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
}
@@ -526,7 +386,7 @@
//update crc_chk from buffer
for(int i = 0; i < RX_LEN; i++) {((uint16_t*)(&tmp_crc_chk))[i] = rx_buff[i];}
// CHECK THE CHECKSUM
- uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 48);
+ uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 12); //DONT CRC THE CRC!!!
// READ CHECKSUM
uint32_t _rx_crc = tmp_crc_chk.checksum;
@@ -538,13 +398,11 @@
//process();
control();
PackAll();
- WriteAll();
+ WriteAll();
//new_command = 1;
- //print_SPI_command();
+ //print_SPI_command();
}
else{pc.printf("bigger F in the chat...\n");}
-
-
}
int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
@@ -579,27 +437,9 @@
//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);
can2.write(q21_can);
- EnterMotorMode(&q22_can);
- can2.write(q22_can);
- EnterMotorMode(&q23_can);
- can2.write(q23_can);
- //BUS THREE
- EnterMotorMode(&q31_can);
- can3.write(q31_can);
- EnterMotorMode(&q32_can);
- can3.write(q32_can);
- EnterMotorMode(&q33_can);
- can3.write(q33_can);
- //WRITE THE COMMANDS
- //WriteAll();
- //SERIAL TO USER
//pc.printf("e\n\r");
return;
}
@@ -608,70 +448,29 @@
//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);
can2.write(q21_can);
- ExitMotorMode(&q22_can);
- can2.write(q22_can);
- ExitMotorMode(&q23_can);
- can2.write(q23_can);
- //BUS THREE
- ExitMotorMode(&q31_can);
- can3.write(q31_can);
- ExitMotorMode(&q32_can);
- can3.write(q32_can);
- ExitMotorMode(&q33_can);
- can3.write(q33_can);
- //WRITE THE COMMANDS
- //WriteAll();
- //SERIAL TO USER
- //pc.printf("x\n\r");
return;
}
//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;
spi_data.tau_1s[0] = g1_state.a1.t;
- spi_data.tau_2s[0] = g1_state.a2.t;
- spi_data.tau_3s[0] = g1_state.a3.t;
//BUS 2 DATA
spi_data.q_1s[1] = g2_state.a1.p;
- spi_data.q_2s[1] = g2_state.a2.p;
- spi_data.q_3s[1] = g2_state.a3.p;
spi_data.qd_1s[1] = g2_state.a1.v;
- spi_data.qd_2s[1] = g2_state.a2.v;
- spi_data.qd_3s[1] = g2_state.a3.v;
spi_data.tau_1s[1] = g2_state.a1.t;
- spi_data.tau_2s[1] = g2_state.a2.t;
- spi_data.tau_3s[1] = g2_state.a3.t;
- //BUS 3 DATA
- spi_data.q_1s[2] = g3_state.a1.p;
- spi_data.q_2s[2] = g3_state.a2.p;
- spi_data.q_3s[2] = g3_state.a3.p;
- spi_data.qd_1s[2] = g3_state.a1.v;
- spi_data.qd_2s[2] = g3_state.a2.v;
- spi_data.qd_3s[2] = g3_state.a3.v;
- spi_data.tau_1s[2] = g3_state.a1.t;
- spi_data.tau_2s[2] = g3_state.a2.t;
- spi_data.tau_3s[2] = g3_state.a3.t;
if(estop==0){
printf("estopped!!!!\n\r");
memset(&g1_control, 0, sizeof(g1_control));
memset(&g2_control, 0, sizeof(g2_control));
- memset(&g3_control, 0, sizeof(g3_control));
+ //memset(&g3_control, 0, sizeof(g3_control));
spi_data.flags[0] = 0xdead;
spi_data.flags[1] = 0xdead;
- spi_data.flags[2] = 0xdead;
+ //spi_data.flags[2] = 0xdead;
led = 1;
}
@@ -679,7 +478,7 @@
led = 0;
memset(&g1_control, 0, sizeof(g1_control));
memset(&g2_control, 0, sizeof(g2_control));
- memset(&g3_control, 0, sizeof(g3_control));
+ //memset(&g3_control, 0, sizeof(g3_control));
//TRANSLATE SPI TO ACTUATOR COMMANNDS
//CAN1
@@ -689,78 +488,17 @@
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];
-
//SPI FLAGS RETURN //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE
spi_data.flags[0] = 0;
spi_data.flags[1] = 0;
- spi_data.flags[2] = 0;
- //spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A1_LIM_P, A1_LIM_N);
- //spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, A2_LIM_P, A2_LIM_N))<<1;
- //spi_data.flags[0] |= (softstop_joint(g1_state.a3, &g1_control.a3, A3_LIM_P, A3_LIM_N))<<2;
- //spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A1_LIM_P, A1_LIM_N);
- //spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, A2_LIM_P, A2_LIM_N))<<1;
- //spi_data.flags[1] |= (softstop_joint(g2_state.a3, &g2_control.a3, A3_LIM_P, A3_LIM_N))<<2;
- //spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A1_LIM_P, A1_LIM_N);
- //spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, A2_LIM_P, A2_LIM_N))<<1;
- //spi_data.flags[2] |= (softstop_joint(g3_state.a3, &g3_control.a3, A3_LIM_P, A3_LIM_N))<<2;
-
- //spi_data.flags[0] = 0xbeef;
- //spi_data.flags[1] = 0xbeef;
- //PackAll();
- //WriteAll();
}
- // UPDATE CHECKSUM
- spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30);
+ spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8);
for(int i = 0; i < DATA_LEN; i++){
tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
@@ -772,19 +510,15 @@
for(int i = 0; i < 3; i++)
{
spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f;
- spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f;
- spi_data.q_3s[i] = spi_command.q_des_3s[i] + 1.f;
spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f;
- spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f;
- spi_data.qd_3s[i] = spi_command.qd_des_3s[i] + 1.f;
}
spi_data.flags[0] = 0xdead;
//spi_data.flags[1] = 0xbeef;
// only do first 56 bytes of message.
- spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
+ spi_data.checksum = xor_checksum((uint32_t*)&spi_data,12);
for(int i = 0; i < DATA_LEN; i++)
tx_buff[i] = ((uint16_t*)(&spi_data))[i];
@@ -793,7 +527,7 @@
void init_spi(void){
SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
spi->format(16, 0);
- spi->frequency(6000000);
+ spi->frequency(500000);
spi->reply(0x0);
cs.fall(&spi_isr);
pc.printf("done\n\r");
@@ -805,18 +539,9 @@
pc.baud(115200); //MAYBE CHANGE THIS IF NEEDED
pc.attach(&serial_isr);
estop.mode(PullUp);
- //spi.format(16, 0);
- //spi.frequency(1000000);
- //spi.reply(0x0);
- //cs.fall(&spi_isr);
- //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
- //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
- can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
memset(&spi_data, 0, sizeof(spi_data_t));
@@ -832,46 +557,21 @@
//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
- q22_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;
- q12_can.id = 0x2;
- q13_can.id = 0x3;
//CAN 2 BUS
q21_can.id = 0x1;
- q22_can.id = 0x2;
- q23_can.id = 0x3;
- //CAN 3 BUS
- q31_can.id = 0x1;
- q32_can.id = 0x2;
- q33_can.id = 0x3;
//actuators on the CAN1 bus
pack_cmd(&q11_can, g1_control.a1);
- pack_cmd(&q12_can, g1_control.a2);
- pack_cmd(&q13_can, g1_control.a3);
//actuators on the CAN2 bus
pack_cmd(&q21_can, g2_control.a1);
- pack_cmd(&q22_can, g2_control.a2);
- pack_cmd(&q23_can, g2_control.a3);
- //actuators on the CAN3 bus
- pack_cmd(&q31_can, g3_control.a1);
- pack_cmd(&q32_can, g3_control.a2);
- pack_cmd(&q33_can, g3_control.a3);
//WRITE THE INITIAL COMMAND
WriteAll();
@@ -891,24 +591,13 @@
//spi_command=set the thing here...
while(1) {
- //pc.printf("test, of SPINE\r\n");
+ //pc.printf("heartbeat...\n");
counter++;
can2.read(rxMsg2);
unpack_reply(rxMsg2, &g2_state);
can1.read(rxMsg1); // read message into Rx message storage
unpack_reply(rxMsg1, &g1_state);
- can3.read(rxMsg3); // read message into Rx message storage
- unpack_reply(rxMsg3, &g3_state);
- wait_us(10);
-
- //print heatbeat (always will print message 0)
- /*
- if (counter3 == 100000) { //for debugging purposes
- pc.printf("HEARTBEAT\n");
- counter3 = 0;
- }
- counter3++;
- */
- }
+ ///wait(0.01);
+ }
}
\ No newline at end of file