Aditya Mehrotra
/
SPIne_Plus_DYNO_SENSORS
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.
Revision 13:ee50f3145be7, committed 2021-07-20
- Comitter:
- adimmit
- Date:
- Tue Jul 20 19:34:50 2021 +0000
- Parent:
- 12:b203f3ae57d0
- Commit message:
- working;
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 Wed May 12 15:11:14 2021 +0000
+++ b/leg_message.h Tue Jul 20 19:34:50 2021 +0000
@@ -3,45 +3,16 @@
#include <stdint.h>
-// 36 bytes
-// 18 16-bit words
+// 24 bytes
+// 12 16-bit words
struct spi_data_t
{
- float q_1s[2];
- float qd_1s[2];
- float tau_1s[2];
- int32_t flags[2];
+ float vol;
+ float lv_current;
+ float hv_current;
+ float ext_current;
+ float tau;
int32_t checksum;
};
-// 52 bytes
-// 26 16-bit words
-struct spi_command_t
-{
- 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];
- int32_t checksum;
-};
-
-
-
-struct joint_control{
- float p_des, v_des, kp, kd, t_ff;
- };
-
-struct joint_state{
- float p, v, t;
- };
-
-struct grouped_act_state{
- joint_state a1; //removed a2 and a3
- };
-struct grouped_act_control{
- joint_control a1;
- }
- ;
#endif
\ No newline at end of file
--- a/main.cpp Wed May 12 15:11:14 2021 +0000
+++ b/main.cpp Tue Jul 20 19:34:50 2021 +0000
@@ -10,44 +10,25 @@
#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
+#define RX_LEN 12 //CHECK THESE BUFFER LENGHTS
+#define TX_LEN 12
// length of outgoing/incoming messages
-#define DATA_LEN 18 //CHECK THESE BUFFER LENGHTS
-#define CMD_LEN 26 //CHECK THESE BUFFER LENGHTS
+#define CMD_LEN 12
+#define DATA_LEN 12 //CHECK THESE BUFFER LENGHTS
// Master CAN ID ///
#define CAN_ID 0x0
/// 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
-
- /// 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 MIN_CUR -400.0f
+#define MAX_CUR 400.0f
-#define ENABLE_CMD 0xFFFF
-#define DISABLE_CMD 0x1F1F
+#define MIN_TAU -5.0f
+#define MAX_TAU 5.0f
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];
@@ -59,11 +40,9 @@
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
+//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
+CANMessage rxMsg1, rxMsg2;
int ledState;
Ticker sendCAN;
int counter = 0;
@@ -73,293 +52,52 @@
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){
+void unpack_reply_4bit(CANMessage msg, spi_data_t * spi_data){
+ //rx2msg is the data from the torque board (CAN2)
+ //rx1msg is the data from the upboard (CAN1)
+
/// 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];
+ uint16_t id = msg.id;
+ uint16_t u1 = (msg.data[0]<<8)|msg.data[1];
+ uint16_t u2 = (msg.data[2]<<8)|msg.data[3];
+
/// 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);
+ float f1 = uint_to_float(u1, MIN_CUR, MAX_CUR, 16);
+ float f2 = uint_to_float(u2, MIN_CUR, MAX_CUR, 16);
if(id==1){
- group->a1.p = p;
- group->a1.v = v;
- group->a1.t = t;
+ spi_data->hv_current = f1;
+ spi_data->ext_current = f2;
+ //pc.printf("ext_current: %f\n", f2);
}
- /*
- else if(id==2){
- group->a2.p = p;
- group->a2.v = v;
- group->a2.t = t;
+ if(id==2){
+ spi_data->vol = f1;
+ spi_data->lv_current = f2;
}
- */
- /*
- else if(id==3){
- group->a3.p = p;
- group->a3.v = v;
- group->a3.t = t;
- }
- */
}
+
+void unpack_reply_2bit(CANMessage msg, spi_data_t * spi_data){
+ //rx2msg is the data from the torque board (CAN2)
+ //rx1msg is the data from the upboard (CAN1)
+
+ /// unpack ints from can buffer ///
+ //uint16_t id = msg.id;
+ uint16_t tau = (msg.data[0]<<8)|msg.data[1];
+ /// convert uints to floats ///
+ float t = uint_to_float(tau, MIN_TAU, MAX_TAU, 16);
+ //pc.printf("torque: %f\n", t);
+ spi_data->tau = t;
+}
void rxISR1() {
can1.read(rxMsg1); // read message into Rx message storage
- unpack_reply(rxMsg1, &g1_state);
+ unpack_reply_4bit(rxMsg1, &spi_data);
}
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();
+ unpack_reply_2bit(rxMsg2, &spi_data);
}
-
-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)
{
@@ -369,105 +107,12 @@
return t;
}
-
-/*
-void print_SPI_command() {
- pc.printf("SPI MESSAGE RECIEVED:\n");
- //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 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 update_buffers(void) {
+ //spi_data.checksum = 100;
+ spi_data.checksum = xor_checksum((uint32_t*)&spi_data,4); //only do first 16 bytes
+ for(int i = 0; i < DATA_LEN; i++){
+ tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
}
-*/
-
-
-/*
-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)
{
@@ -485,290 +130,14 @@
}
}
}
- //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();
-
-
- //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];
+ update_buffers();
}
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(12000000);
spi->reply(0x0);
cs.fall(&spi_isr);
pc.printf("done\n\r");
@@ -778,7 +147,6 @@
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);
@@ -791,71 +159,27 @@
//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(&rx_buff, 0, RX_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");
+ pc.printf("\n\r SPIne SENSOR\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();
+ rxMsg1.len = 4; //receive 4 bytes
+ rxMsg2.len = 2;
//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();
@@ -867,23 +191,16 @@
while(1) {
//pc.printf("test, of SPINE\r\n");
+ //wait(0.0001); //MAKE SURE IT's 1-3k rate, higher we will fill the CAN buffer
+
counter++;
can2.read(rxMsg2);
- unpack_reply(rxMsg2, &g2_state);
+ unpack_reply_2bit(rxMsg2, &spi_data);
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);
+ unpack_reply_4bit(rxMsg1, &spi_data);
+ //wait_us(10);
+ //wait(0.01);
- //print heatbeat (always will print message 0)
- /*
- if (counter3 == 100000) { //for debugging purposes
- pc.printf("HEARTBEAT\n");
- counter3 = 0;
- }
- counter3++;
- */
}
}
\ No newline at end of file