Ben Katz / SPIne

SPI to quad CAN firmware

Dependencies:   mbed-dev_spine

Fork of Teleop_Controller by Ben Katz

main.cpp@0:d6186b8990c5, 2017-05-26 (annotated)

Committer:
benkatz
Date:
Fri May 26 19:47:57 2017 +0000
Revision:
0:d6186b8990c5
Child:
1:79e0d4791936
1st commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 0:d6186b8990c5 2 #define CAN_ID 0x1
benkatz 0:d6186b8990c5 3
benkatz 0:d6186b8990c5 4 #include "mbed.h"
benkatz 0:d6186b8990c5 5 #include "math_ops.h"
benkatz 0:d6186b8990c5 6
benkatz 0:d6186b8990c5 7
benkatz 0:d6186b8990c5 8 Serial pc(PA_2, PA_3);
benkatz 0:d6186b8990c5 9 CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 0:d6186b8990c5 10 CANMessage rxMsg;
benkatz 0:d6186b8990c5 11 CANMessage txMsg1;
benkatz 0:d6186b8990c5 12 CANMessage txMsg2;
benkatz 0:d6186b8990c5 13 int ledState;
benkatz 0:d6186b8990c5 14 Timer timer;
benkatz 0:d6186b8990c5 15 Ticker sendCAN;
benkatz 0:d6186b8990c5 16 int counter = 0;
benkatz 0:d6186b8990c5 17 volatile bool msgAvailable = false;
benkatz 0:d6186b8990c5 18 Ticker loop;
benkatz 0:d6186b8990c5 19
benkatz 0:d6186b8990c5 20 float theta1, theta2, dtheta1, dtheta2;
benkatz 0:d6186b8990c5 21
benkatz 0:d6186b8990c5 22 /// Value Limits ///
benkatz 0:d6186b8990c5 23 #define P_MIN -12.5f
benkatz 0:d6186b8990c5 24 #define P_MAX 12.5f
benkatz 0:d6186b8990c5 25 #define V_MIN -30.0f
benkatz 0:d6186b8990c5 26 #define V_MAX 30.0f
benkatz 0:d6186b8990c5 27 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 28 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 29 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 30 #define KD_MAX 5.0f
benkatz 0:d6186b8990c5 31 #define T_MIN -18.0f
benkatz 0:d6186b8990c5 32 #define T_MAX 18.0f
benkatz 0:d6186b8990c5 33 #define I_MAX 40.0f
benkatz 0:d6186b8990c5 34
benkatz 0:d6186b8990c5 35 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 36 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 37 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 38 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 39 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 40 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 41 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 42 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 43 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 44 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 45 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 46 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 47 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 48 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 49 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 50 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 51
benkatz 0:d6186b8990c5 52 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){
benkatz 0:d6186b8990c5 53 /// limit data to be within bounds ///
benkatz 0:d6186b8990c5 54 p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
benkatz 0:d6186b8990c5 55 v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
benkatz 0:d6186b8990c5 56 kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
benkatz 0:d6186b8990c5 57 kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
benkatz 0:d6186b8990c5 58 t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
benkatz 0:d6186b8990c5 59 /// convert floats to unsigned ints ///
benkatz 0:d6186b8990c5 60 int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 61 int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 62 int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 0:d6186b8990c5 63 int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 0:d6186b8990c5 64 int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
benkatz 0:d6186b8990c5 65 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 66 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 67 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 68 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 69 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 70 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 71 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 72 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 73 msg->data[7] = t_int&0xff;
benkatz 0:d6186b8990c5 74 }
benkatz 0:d6186b8990c5 75
benkatz 0:d6186b8990c5 76 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 77 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 78 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 79 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 80 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 81 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 82 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 83 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 84 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 85 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 86 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 87
benkatz 0:d6186b8990c5 88 void unpack_reply(CANMessage msg){
benkatz 0:d6186b8990c5 89 /// unpack ints from can buffer ///
benkatz 0:d6186b8990c5 90 int id = msg.data[0];
benkatz 0:d6186b8990c5 91 int p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 0:d6186b8990c5 92 int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 0:d6186b8990c5 93 int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 0:d6186b8990c5 94 /// convert ints to floats ///
benkatz 0:d6186b8990c5 95 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 96 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 97 float i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
benkatz 0:d6186b8990c5 98
benkatz 0:d6186b8990c5 99 if(id == 2){
benkatz 0:d6186b8990c5 100 theta1 = p;
benkatz 0:d6186b8990c5 101 dtheta1 = v;
benkatz 0:d6186b8990c5 102 }
benkatz 0:d6186b8990c5 103 else if(id ==3){
benkatz 0:d6186b8990c5 104 theta2 = p;
benkatz 0:d6186b8990c5 105 dtheta2 = v;
benkatz 0:d6186b8990c5 106 }
benkatz 0:d6186b8990c5 107
benkatz 0:d6186b8990c5 108 }
benkatz 0:d6186b8990c5 109
benkatz 0:d6186b8990c5 110 void onMsgReceived() {
benkatz 0:d6186b8990c5 111 can.read(rxMsg); // read message into Rx message storage
benkatz 0:d6186b8990c5 112 unpack_reply(rxMsg);
benkatz 0:d6186b8990c5 113 }
benkatz 0:d6186b8990c5 114
benkatz 0:d6186b8990c5 115
benkatz 0:d6186b8990c5 116 void sendCMD(){
benkatz 0:d6186b8990c5 117 /// bilateral teleoperation demo ///
benkatz 0:d6186b8990c5 118 pack_cmd(&txMsg1, theta2, dtheta2, 10, .1, 0);
benkatz 0:d6186b8990c5 119 pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
benkatz 0:d6186b8990c5 120 can.write(txMsg2);
benkatz 0:d6186b8990c5 121 wait(.0003); // Give motor 1 time to respond.
benkatz 0:d6186b8990c5 122 can.write(txMsg1);
benkatz 0:d6186b8990c5 123 }
benkatz 0:d6186b8990c5 124
benkatz 0:d6186b8990c5 125 void serial_isr(){
benkatz 0:d6186b8990c5 126 /// hangle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 127 while(pc.readable()){
benkatz 0:d6186b8990c5 128 char c = pc.getc();
benkatz 0:d6186b8990c5 129 switch(c){
benkatz 0:d6186b8990c5 130 case(27):
benkatz 0:d6186b8990c5 131 printf("\n\r exiting motor mode \n\r");
benkatz 0:d6186b8990c5 132 txMsg1.data[0] = 0xFF;
benkatz 0:d6186b8990c5 133 txMsg1.data[1] = 0xFF;
benkatz 0:d6186b8990c5 134 txMsg1.data[2] = 0xFF;
benkatz 0:d6186b8990c5 135 txMsg1.data[3] = 0xFF;
benkatz 0:d6186b8990c5 136 txMsg1.data[4] = 0xFF;
benkatz 0:d6186b8990c5 137 txMsg1.data[5] = 0xFF;
benkatz 0:d6186b8990c5 138 txMsg1.data[6] = 0xFF;
benkatz 0:d6186b8990c5 139 txMsg1.data[7] = 0xFD;
benkatz 0:d6186b8990c5 140
benkatz 0:d6186b8990c5 141 txMsg2.data[0] = 0xFF;
benkatz 0:d6186b8990c5 142 txMsg2.data[1] = 0xFF;
benkatz 0:d6186b8990c5 143 txMsg2.data[2] = 0xFF;
benkatz 0:d6186b8990c5 144 txMsg2.data[3] = 0xFF;
benkatz 0:d6186b8990c5 145 txMsg2.data[4] = 0xFF;
benkatz 0:d6186b8990c5 146 txMsg2.data[5] = 0xFF;
benkatz 0:d6186b8990c5 147 txMsg2.data[6] = 0xFF;
benkatz 0:d6186b8990c5 148 txMsg2.data[7] = 0xFD;
benkatz 0:d6186b8990c5 149 break;
benkatz 0:d6186b8990c5 150 case('m'):
benkatz 0:d6186b8990c5 151 printf("\n\r entering motor mode \n\r");
benkatz 0:d6186b8990c5 152 txMsg1.data[0] = 0xFF;
benkatz 0:d6186b8990c5 153 txMsg1.data[1] = 0xFF;
benkatz 0:d6186b8990c5 154 txMsg1.data[2] = 0xFF;
benkatz 0:d6186b8990c5 155 txMsg1.data[3] = 0xFF;
benkatz 0:d6186b8990c5 156 txMsg1.data[4] = 0xFF;
benkatz 0:d6186b8990c5 157 txMsg1.data[5] = 0xFF;
benkatz 0:d6186b8990c5 158 txMsg1.data[6] = 0xFF;
benkatz 0:d6186b8990c5 159 txMsg1.data[7] = 0xFC;
benkatz 0:d6186b8990c5 160
benkatz 0:d6186b8990c5 161 txMsg2.data[0] = 0xFF;
benkatz 0:d6186b8990c5 162 txMsg2.data[1] = 0xFF;
benkatz 0:d6186b8990c5 163 txMsg2.data[2] = 0xFF;
benkatz 0:d6186b8990c5 164 txMsg2.data[3] = 0xFF;
benkatz 0:d6186b8990c5 165 txMsg2.data[4] = 0xFF;
benkatz 0:d6186b8990c5 166 txMsg2.data[5] = 0xFF;
benkatz 0:d6186b8990c5 167 txMsg2.data[6] = 0xFF;
benkatz 0:d6186b8990c5 168 txMsg2.data[7] = 0xFC;
benkatz 0:d6186b8990c5 169 break;
benkatz 0:d6186b8990c5 170 case('z'):
benkatz 0:d6186b8990c5 171 printf("\n\r zeroing \n\r");
benkatz 0:d6186b8990c5 172 txMsg1.data[0] = 0xFF;
benkatz 0:d6186b8990c5 173 txMsg1.data[1] = 0xFF;
benkatz 0:d6186b8990c5 174 txMsg1.data[2] = 0xFF;
benkatz 0:d6186b8990c5 175 txMsg1.data[3] = 0xFF;
benkatz 0:d6186b8990c5 176 txMsg1.data[4] = 0xFF;
benkatz 0:d6186b8990c5 177 txMsg1.data[5] = 0xFF;
benkatz 0:d6186b8990c5 178 txMsg1.data[6] = 0xFF;
benkatz 0:d6186b8990c5 179 txMsg1.data[7] = 0xFE;
benkatz 0:d6186b8990c5 180
benkatz 0:d6186b8990c5 181 txMsg2.data[0] = 0xFF;
benkatz 0:d6186b8990c5 182 txMsg2.data[1] = 0xFF;
benkatz 0:d6186b8990c5 183 txMsg2.data[2] = 0xFF;
benkatz 0:d6186b8990c5 184 txMsg2.data[3] = 0xFF;
benkatz 0:d6186b8990c5 185 txMsg2.data[4] = 0xFF;
benkatz 0:d6186b8990c5 186 txMsg2.data[5] = 0xFF;
benkatz 0:d6186b8990c5 187 txMsg2.data[6] = 0xFF;
benkatz 0:d6186b8990c5 188 txMsg2.data[7] = 0xFE;
benkatz 0:d6186b8990c5 189 break;
benkatz 0:d6186b8990c5 190 }
benkatz 0:d6186b8990c5 191 }
benkatz 0:d6186b8990c5 192 can.write(txMsg1);
benkatz 0:d6186b8990c5 193 can.write(txMsg2);
benkatz 0:d6186b8990c5 194
benkatz 0:d6186b8990c5 195 }
benkatz 0:d6186b8990c5 196
benkatz 0:d6186b8990c5 197 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
benkatz 0:d6186b8990c5 198 int main() {
benkatz 0:d6186b8990c5 199 pc.baud(921600);
benkatz 0:d6186b8990c5 200 pc.attach(&serial_isr);
benkatz 0:d6186b8990c5 201 can.frequency(1000000); // set bit rate to 1Mbps
benkatz 0:d6186b8990c5 202 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 0:d6186b8990c5 203 can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 0:d6186b8990c5 204 printf("Master\n\r");
benkatz 0:d6186b8990c5 205 //printf("%d\n\r", RX_ID << 18);
benkatz 0:d6186b8990c5 206 int count = 0;
benkatz 0:d6186b8990c5 207 txMsg1.len = 8; //transmit 8 bytes
benkatz 0:d6186b8990c5 208 txMsg2.len = 8; //transmit 8 bytes
benkatz 0:d6186b8990c5 209 rxMsg.len = 6; //receive 5 bytes
benkatz 0:d6186b8990c5 210 loop.attach(&sendCMD, .001);
benkatz 0:d6186b8990c5 211 txMsg1.id = 0x2; //1st motor ID
benkatz 0:d6186b8990c5 212 txMsg2.id = 0x3; //2nd motor ID
benkatz 0:d6186b8990c5 213 pack_cmd(&txMsg1, 0, 0, 0, 0, 0); //Start out by sending all 0's
benkatz 0:d6186b8990c5 214 pack_cmd(&txMsg2, 0, 0, 0, 0, 0);
benkatz 0:d6186b8990c5 215 timer.start();
benkatz 0:d6186b8990c5 216
benkatz 0:d6186b8990c5 217 while(1) {
benkatz 0:d6186b8990c5 218
benkatz 0:d6186b8990c5 219 }
benkatz 0:d6186b8990c5 220
benkatz 0:d6186b8990c5 221 }
benkatz 0:d6186b8990c5 222
benkatz 0:d6186b8990c5 223
benkatz 0:d6186b8990c5 224
benkatz 0:d6186b8990c5 225
benkatz 0:d6186b8990c5 226