Aditya Mehrotra
/
SPIne_Plus_DYNO
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 15:d3c93f01a02b
- Parent:
- 14:8a7fd1ebc209
- Child:
- 16:8f8adc5f8f0a
--- a/main.cpp Mon Aug 30 15:47:18 2021 +0000
+++ b/main.cpp Mon Jan 31 20:43:05 2022 +0000
@@ -1,6 +1,5 @@
-//counter for misc purposes
-int counter3 = 0;
+
//GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!!
//ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING
@@ -9,359 +8,25 @@
#include <cstring>
#include "leg_message.h"
-// length of receive/transmit buffers
-#define RX_LEN 26 //CHECK THESE BUFFER LENGHTS
-#define TX_LEN 26 //CHECK THESE BUFFER LENGHTS
-
-// length of outgoing/incoming messages
-#define DATA_LEN 18 //CHECK THESE BUFFER LENGHTS
-#define CMD_LEN 26 //CHECK THESE BUFFER LENGHTS
-
-// Master CAN ID ///
-#define CAN_ID 0x0
-
+#define BYTES 196
+#define RX_LEN 196
+#define TX_LEN 124
-/// Value Limits ///
- #define P_MIN -12.5f
- #define P_MAX 12.5f
- #define V_MIN -65.0f
- #define V_MAX 65.0f
- #define KP_MIN 0.0f
- #define KP_MAX 500.0f
- #define KD_MIN 0.0f
- #define KD_MAX 10.0f
- #define T_MIN -72.0f
- #define T_MAX 72.0f
+// spi buffers
+uint8_t rx_buff[RX_LEN];
+uint8_t tx_buff[TX_LEN];
-
- /// Joint Soft Stops ///
- #define A1_LIM_P 1.5f
- #define A1_LIM_N -1.5f
- #define A2_LIM_P 1.5f
- #define A2_LIM_N -1.5f
- #define A3_LIM_P 1.5f
- #define A3_LIM_N -1.5f
- #define KP_SOFTSTOP 100.0f
- #define KD_SOFTSTOP 0.4f;
-
-#define ENABLE_CMD 0xFFFF
-#define DISABLE_CMD 0x1F1F
-
+// vars
spi_data_t spi_data; // data from spine to up
spi_command_t spi_command; // data from up to spine
-
-// spi buffers
-uint16_t rx_buff[RX_LEN];
-uint16_t tx_buff[TX_LEN];
-
-DigitalOut led(PC_5);
-
+spi_command_t tmp_crc_chk; // data from up to spine
Serial pc(PA_2, PA_3);
-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; //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;
-volatile bool msgAvailable = false;
-Ticker loop;
int spi_enabled = 0;
InterruptIn cs(PA_4);
-DigitalIn estop(PB_15);
-//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;
-
-uint16_t x = 0;
-uint16_t x2 = 0;
-uint16_t count = 0;
-uint16_t counter2 = 0; //SEE IF WE NEED TO UPDATE THESE TO ADD COUNTER3 AND X3
-
-int control_mode = 1;
-int is_standing = 0; //SEE IF WE STILL NEED THE STANDING THING
-int enabled = 0;
-
-// generates fake spi data from spi command
-void test_control(); //MAY NEED TO GET RID OF THIS?
-void control();
-
-
-/// CAN Command Packet Structure ///
-/// 16 bit position command, between -4*pi and 4*pi
-/// 12 bit velocity command, between -30 and + 30 rad/s
-/// 12 bit kp, between 0 and 500 N-m/rad
-/// 12 bit kd, between 0 and 100 N-m*s/rad
-/// 12 bit feed forward torque, between -18 and 18 N-m
-/// CAN Packet is 8 8-bit words
-/// Formatted as follows. For each quantity, bit 0 is LSB
-/// 0: [position[15-8]]
-/// 1: [position[7-0]]
-/// 2: [velocity[11-4]]
-/// 3: [velocity[3-0], kp[11-8]]
-/// 4: [kp[7-0]]
-/// 5: [kd[11-4]]
-/// 6: [kd[3-0], torque[11-8]]
-/// 7: [torque[7-0]]
-
-void pack_cmd(CANMessage * msg, joint_control joint){
-
- /// limit data to be within bounds ///
- float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);
- float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
- float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
- float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
- float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
- /// convert floats to unsigned ints ///
- uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
- uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
- uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
- uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
- uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
- /// pack ints into the can buffer ///
- msg->data[0] = p_int>>8;
- msg->data[1] = p_int&0xFF;
- msg->data[2] = v_int>>4;
- msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
- msg->data[4] = kp_int&0xFF;
- msg->data[5] = kd_int>>4;
- msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
- msg->data[7] = t_int&0xff;
- }
-
-/// CAN Reply Packet Structure ///
-/// 16 bit position, between -4*pi and 4*pi
-/// 12 bit velocity, between -30 and + 30 rad/s
-/// 12 bit current, between -40 and 40;
-/// CAN Packet is 5 8-bit words
-/// Formatted as follows. For each quantity, bit 0 is LSB
-/// 0: [position[15-8]]
-/// 1: [position[7-0]]
-/// 2: [velocity[11-4]]
-/// 3: [velocity[3-0], current[11-8]]
-/// 4: [current[7-0]]
-
-void unpack_reply(CANMessage msg, grouped_act_state * group){
- /// 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){
- group->a1.p = p;
- 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;
- }
- */
- }
+SPISlave spi(PA_7, PA_6, PA_5, PA_4);
- void rxISR1() {
- can1.read(rxMsg1); // read message into Rx message storage
- unpack_reply(rxMsg1, &g1_state);
-}
-void rxISR2(){
- 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;
- //ID = 1 actuators
- can1.write(q11_can);
- 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(){
- counter ++;
-
- 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);
- counter = 0 ;
- }
-
- WriteAll();
- }
-
-
-
-
-void Zero(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] = 0xFE;
- //WriteAll();
- }
-
-void EnterMotorMode(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] = 0xFC;
- //WriteAll();
- }
-
-void ExitMotorMode(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] = 0xFD;
- //WriteAll();
- }
-
-
-void serial_isr(){
- /// handle keyboard commands from the serial terminal ///
- while(pc.readable()){
- char c = pc.getc();
- //led = !led;
- switch(c){
- 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(&q22_can);
- //ExitMotorMode(&q23_can);
- //CAN BUS 3
- //ExitMotorMode(&q31_can);
- //ExitMotorMode(&q32_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(&q22_can);
- //EnterMotorMode(&q23_can);
- //CAN BUS 3
- //EnterMotorMode(&q31_can);
- //EnterMotorMode(&q32_can);
- //EnterMotorMode(&q33_can);
- //WAIT FOR ENABLE
- wait(.5);
- //ENABLE FLAG
- enabled = 1;
- //loop.attach(&sendCMD, .001);
- break;
- case('s'):
- pc.printf("\n\r standing \n\r");
- counter2 = 0;
- is_standing = 1;
- //stand();
- break;
- case('z'):
- pc.printf("\n\r zeroing \n\r");
- //CAN BUS 1
- Zero(&q11_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(&q33_can);
- break;
- }
- }
- WriteAll();
-
- }
-
-
uint32_t xor_checksum(uint32_t* data, size_t len)
{
uint32_t t = 0;
@@ -370,508 +35,112 @@
return t;
}
-
-
-void print_SPI_command() {
- pc.printf("SPI MESSAGE RECIEVED:\n");
- 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]);
-
+void process()
+{
+ // 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); //NOTE, CHECK THIS WE WANT TO DO IT ON THE FIRST 16 SPI BYTES
}
-
-/*
-void print_SPI_data() {
- pc.printf("SPI MESSAGE SENT:\n");
- //CAN ONE
- 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]);
-
-}
-*/
-
-
-
-
void spi_isr(void)
{
- //pc.printf("CS ACTIVE\n");
- GPIOC->ODR |= (1 << 8);
- GPIOC->ODR &= ~(1 << 8);
int bytecount = 0;
- SPI1->DR = tx_buff[0];
- while(cs == 0) {
- if(SPI1->SR&0x1) {
- rx_buff[bytecount] = SPI1->DR;
+ //spi.reply(tx_buff[0]);
+ while (cs==0){
+ if(spi.receive()) {
+ rx_buff[bytecount] = spi.read();
+ if (bytecount<TX_LEN) {spi.reply(tx_buff[bytecount]);}
bytecount++;
- if(bytecount<TX_LEN) {
- SPI1->DR = tx_buff[bytecount];
- }
}
}
- //pc.printf("RECIEVED: %d BYTES\n", bytecount);
-
- //pc.printf("HERE'S A SPI COMMAND:\n");
- // after reading, save into spi_command
- // should probably check checksum first!
- uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
- for(int i = 0; i < CMD_LEN; i++)
- {
- ((uint16_t*)(&spi_command))[i] = rx_buff[i];
- //pc.printf("BYTE %d SENT: %d\n", i, tx_buff[i]);
- //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]);
- }
- //print_SPI_command();
-
- // run control, which fills in tx_buff for the next iteration
- /*
- if(calc_checksum != spi_command.checksum){
- spi_data.flags[1] = 0xdead;
- pc.printf("FAILED CHECKSUM\n");
- pc.printf("ACTUAL: %d\n", calc_checksum);
- pc.printf("CURRENT: %d\n", spi_command.checksum);}
- */
-
- //print_SPI_data();
- control();
- PackAll();
- WriteAll();
+
+ //update crc_chk from buffer
+ for(int i = 0; i < RX_LEN; i++) {((uint8_t*)(&tmp_crc_chk))[i] = rx_buff[i];}
+ // CHECK THE CHECKSUM
+ uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 48);
+ // READ CHECKSUM
+ uint32_t _rx_crc = tmp_crc_chk.checksum;
+
+ if(_crc == _rx_crc) {
+ //pc.printf("CHECKSUM PASSED...");
+ //update crc_chk from buffer
+ spi_command = tmp_crc_chk;
+ //do math on the input
+ process();
+ //populate the output
+ for(int i = 0; i < TX_LEN; i++) {tx_buff[i] = ((uint8_t*)(&spi_data))[i];}
+ }
+ else {pc.printf("CRC FAILED...");}
+}
- //for (int i = 0; i<TX_LEN; i++) {
- // tx_buff[i] = 2*rx_buff[i];
- //}
-// for (int i=0; i<TX_LEN; i++) {
-// //printf("%d ", rx_buff[i]);
-// }
- //printf("\n\r");
-}
-
-int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
- /*
- if((state.p)>=limit_p){
- //control->p_des = limit_p;
- control->v_des = 0.0f;
- control->kp = 0;
- control->kd = KD_SOFTSTOP;
- control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
- return 1;
- }
- else if((state.p)<=limit_n){
- //control->p_des = limit_n;
- control->v_des = 0.0f;
- control->kp = 0;
- control->kd = KD_SOFTSTOP;
- control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
- return 1;
- }
- */
- return 0;
-
- }
-
-
-void control()
-{
-
- if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){
- enabled = 1;
- //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;
- }
- else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){
- enabled = 0;
- //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));
- spi_data.flags[0] = 0xdead;
- spi_data.flags[1] = 0xdead;
- //spi_data.flags[2] = 0xdead;
- led = 1;
- }
-
- else{
- led = 0;
- memset(&g1_control, 0, sizeof(g1_control));
- memset(&g2_control, 0, sizeof(g2_control));
- //memset(&g3_control, 0, sizeof(g3_control));
-
- //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];
-
- //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();
- }
- 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];}
-
-}
-
-
-void test_control()
-{
- 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);
-
- for(int i = 0; i < DATA_LEN; i++)
- tx_buff[i] = ((uint16_t*)(&spi_data))[i];
-}
-
-void init_spi(void){
- SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
- spi->format(16, 0);
- spi->frequency(12000000);
- spi->reply(0x0);
- cs.fall(&spi_isr);
- pc.printf("done\n\r");
-}
-
int main() {
wait(1);
- //led = 1;
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));
- memset(&spi_command,0,sizeof(spi_command_t));
-
-
- NVIC_SetPriority(TIM5_IRQn, 1);
- //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
- //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
-
- 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
- //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();
+ memset(&tx_buff, 0, RX_LEN * sizeof(uint8_t));
+ memset(&rx_buff, 0, TX_LEN * sizeof(uint8_t));
//just debugging things
pc.printf("SETUP VARS ALL DONE\n");
-
// SPI doesn't work if enabled while the CS pin is pulled low
// Wait for CS to not be low, then enable SPI
if(!spi_enabled){
- while((spi_enabled==0) && (cs.read() ==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
- init_spi();
+ while((spi_enabled==0) && (cs.read()==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
+ spi.format(8, 0);
+ spi.frequency(500000);
+ spi.reply(0x0);
spi_enabled = 1;
+ cs.fall(&spi_isr);
pc.printf("SPI ENABLED AND READY\n");
}
- //spi_command=set the thing here...
-
+ //initialize the tx_buff
+ for(int i = 0; i < TX_LEN; i++) {
+ tx_buff[i] = ((uint8_t*)(&spi_data))[i];
+ }
+ //initialize the tx_buff
+ for(int i = 0; i < RX_LEN; i++) {
+ tx_buff[i] = ((uint8_t*)(&spi_command))[i];
+ }
+
while(1) {
- //pc.printf("test, of SPINE\r\n");
- counter++;
- can2.read(rxMsg2);
- unpack_reply(rxMsg2, &g2_state);
- can1.read(rxMsg1); // read message into Rx message storage
- unpack_reply(rxMsg1, &g1_state);
- ///wait(0.01);
+ //do a thing
}
}
\ No newline at end of file