Ben Katz
SPI to quad CAN firmware
Fork of
Teleop_Controller
by 
Ben Katz
Diff: main.cpp
- Revision:
- 0:d6186b8990c5
- Child:
- 1:79e0d4791936
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 26 19:47:57 2017 +0000
@@ -0,0 +1,226 @@
+
+#define CAN_ID 0x1
+
+#include "mbed.h"
+#include "math_ops.h"
+
+
+Serial pc(PA_2, PA_3);
+CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
+CANMessage rxMsg;
+CANMessage txMsg1;
+CANMessage txMsg2;
+int ledState;
+Timer timer;
+Ticker sendCAN;
+int counter = 0;
+volatile bool msgAvailable = false;
+Ticker loop;
+
+ float theta1, theta2, dtheta1, dtheta2;
+
+/// Value Limits ///
+ #define P_MIN -12.5f
+ #define P_MAX 12.5f
+ #define V_MIN -30.0f
+ #define V_MAX 30.0f
+ #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 I_MAX 40.0f
+
+/// 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, float p_des, float v_des, float kp, float kd, float t_ff){
+ /// limit data to be within bounds ///
+ p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
+ v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
+ kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
+ kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
+ t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
+ /// convert floats to unsigned ints ///
+ int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
+ int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
+ int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
+ int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
+ int 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){
+ /// unpack ints from can buffer ///
+ int id = msg.data[0];
+ int p_int = (msg.data[1]<<8)|msg.data[2];
+ int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
+ int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
+ /// convert ints 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 i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
+
+ if(id == 2){
+ theta1 = p;
+ dtheta1 = v;
+ }
+ else if(id ==3){
+ theta2 = p;
+ dtheta2 = v;
+ }
+
+ }
+
+ void onMsgReceived() {
+ can.read(rxMsg); // read message into Rx message storage
+ unpack_reply(rxMsg);
+}
+
+
+void sendCMD(){
+ /// bilateral teleoperation demo ///
+ pack_cmd(&txMsg1, theta2, dtheta2, 10, .1, 0);
+ pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
+ can.write(txMsg2);
+ wait(.0003); // Give motor 1 time to respond.
+ can.write(txMsg1);
+ }
+
+void serial_isr(){
+ /// hangle keyboard commands from the serial terminal ///
+ while(pc.readable()){
+ char c = pc.getc();
+ switch(c){
+ case(27):
+ printf("\n\r exiting motor mode \n\r");
+ txMsg1.data[0] = 0xFF;
+ txMsg1.data[1] = 0xFF;
+ txMsg1.data[2] = 0xFF;
+ txMsg1.data[3] = 0xFF;
+ txMsg1.data[4] = 0xFF;
+ txMsg1.data[5] = 0xFF;
+ txMsg1.data[6] = 0xFF;
+ txMsg1.data[7] = 0xFD;
+
+ txMsg2.data[0] = 0xFF;
+ txMsg2.data[1] = 0xFF;
+ txMsg2.data[2] = 0xFF;
+ txMsg2.data[3] = 0xFF;
+ txMsg2.data[4] = 0xFF;
+ txMsg2.data[5] = 0xFF;
+ txMsg2.data[6] = 0xFF;
+ txMsg2.data[7] = 0xFD;
+ break;
+ case('m'):
+ printf("\n\r entering motor mode \n\r");
+ txMsg1.data[0] = 0xFF;
+ txMsg1.data[1] = 0xFF;
+ txMsg1.data[2] = 0xFF;
+ txMsg1.data[3] = 0xFF;
+ txMsg1.data[4] = 0xFF;
+ txMsg1.data[5] = 0xFF;
+ txMsg1.data[6] = 0xFF;
+ txMsg1.data[7] = 0xFC;
+
+ txMsg2.data[0] = 0xFF;
+ txMsg2.data[1] = 0xFF;
+ txMsg2.data[2] = 0xFF;
+ txMsg2.data[3] = 0xFF;
+ txMsg2.data[4] = 0xFF;
+ txMsg2.data[5] = 0xFF;
+ txMsg2.data[6] = 0xFF;
+ txMsg2.data[7] = 0xFC;
+ break;
+ case('z'):
+ printf("\n\r zeroing \n\r");
+ txMsg1.data[0] = 0xFF;
+ txMsg1.data[1] = 0xFF;
+ txMsg1.data[2] = 0xFF;
+ txMsg1.data[3] = 0xFF;
+ txMsg1.data[4] = 0xFF;
+ txMsg1.data[5] = 0xFF;
+ txMsg1.data[6] = 0xFF;
+ txMsg1.data[7] = 0xFE;
+
+ txMsg2.data[0] = 0xFF;
+ txMsg2.data[1] = 0xFF;
+ txMsg2.data[2] = 0xFF;
+ txMsg2.data[3] = 0xFF;
+ txMsg2.data[4] = 0xFF;
+ txMsg2.data[5] = 0xFF;
+ txMsg2.data[6] = 0xFF;
+ txMsg2.data[7] = 0xFE;
+ break;
+ }
+ }
+ can.write(txMsg1);
+ can.write(txMsg2);
+
+ }
+
+int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
+int main() {
+ pc.baud(921600);
+ pc.attach(&serial_isr);
+ can.frequency(1000000); // set bit rate to 1Mbps
+ can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
+ can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+ printf("Master\n\r");
+ //printf("%d\n\r", RX_ID << 18);
+ int count = 0;
+ txMsg1.len = 8; //transmit 8 bytes
+ txMsg2.len = 8; //transmit 8 bytes
+ rxMsg.len = 6; //receive 5 bytes
+ loop.attach(&sendCMD, .001);
+ txMsg1.id = 0x2; //1st motor ID
+ txMsg2.id = 0x3; //2nd motor ID
+ pack_cmd(&txMsg1, 0, 0, 0, 0, 0); //Start out by sending all 0's
+ pack_cmd(&txMsg2, 0, 0, 0, 0, 0);
+ timer.start();
+
+ while(1) {
+
+ }
+
+ }
+
+
+
+
+