Ben Katz
MotorModule CAN example
Diff: main.cpp
- Revision:
- 2:36a254d3dbf3
- Parent:
- 1:d24fd64d1fcb
- Child:
- 3:f0d054d896f9
--- a/main.cpp Sun Jun 30 02:04:46 2019 +0000
+++ b/main.cpp Thu Aug 08 17:03:30 2019 +0000
@@ -1,227 +1,104 @@
-#define CAN_ID 0x1
+#define CAN_ID 0x0
#include "mbed.h"
#include "math_ops.h"
+#include "MotorModule.h"
+
-Serial pc(PA_2, PA_3);
+Serial pc(PA_2, PA_3); // Serial port to the computer
CAN can(PB_8, PB_9, 1000000); // 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;
+Ticker loop; // Control loop interrupt handler
+int loop_counter = 0;
+#define DT .01 // Control loop period
-/// Value Limits ///
-#define P_MIN -12.5f
- #define P_MAX 12.5f
- #define V_MIN -45.0f
- #define V_MAX 45.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
-
-/// 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]]
+#define N_MOTORS 2 // Number of motors on the can bus
+MotorStruct motors[N_MOTORS]; // Create a list of the motors attached
+
+/* Communication functions. Do not touch */
+void onMsgReceived();
+void init_motors(int ids[N_MOTORS]);
+/* */
-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);
+void control()
+{
+ /* Your control loop goes here. */
+ /* Update torques, position/velocity setpoints, etc */
+ motors[0].control.v_des = 5.0f*sin(.01f*loop_counter);
+ motors[0].control.kd = .5f;
+ /* */
- if(id == 2){
- theta1 = p;
- dtheta1 = v;
- }
- else if(id ==3){
- theta2 = p;
- dtheta2 = v;
- }
+ for(int i = 0; i<N_MOTORS; i++)
+ {
+ pack_cmd(&motors[i]);
+ can.write(motors[i].txMsg);
+ }
- //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i);
- }
- void onMsgReceived() {
- can.read(rxMsg); // read message into Rx message storage
- unpack_reply(rxMsg);
+ printf("%f\n\r", motors[0].state.velocity); // This will print to the computer. Usefull for debugging
+ loop_counter++; // Increment loop counter
}
-void sendCMD(){
- /// bilateral teleoperation demo ///
- pack_cmd(&txMsg1, 0, 0, 0, 0, -0.1f);
- //pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
- //pack_cmd(&txMsg2, 0, 0, 0, 0, 1.0);
- //pack_cmd(&txMsg1, 0, 0, 0, 0, 1.0);
- //can.write(txMsg2);
- wait(.0003); // Give motor 1 time to respond.
- can.write(txMsg1);
- }
+int main()
+{
+
+ pc.baud(921600); // Set baud rate for communication over USB serial
+ can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
+ can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
+
+ int ids[N_MOTORS] = {1, 2}; // List of motor CAN ID's
+ init_motors(ids); // Initialize the list of motors
-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);
+ enable_motor(&motors[0], &can); // Enable first motor
+ wait(1); // Wait 1 second
+ disable_motor(&motors[0], &can); // Disable first motor
+
+ loop.attach(&control, DT); // Start running the contorl interrupt at 1/DT Hz
+ while(1)
+ {
+ // Usuallly nothing should run here. Instead run control in the interrupt.
}
-
-int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
-int main() {
- pc.baud(921600);
- pc.attach(&serial_isr);
- 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) {
-
- }
- }
+}
+/* low-level communication functoins below. Do not touch */
+ void onMsgReceived()
+/* This interrupt gets called when a CAN message with ID CAN_ID shows up */
+{
+ CANMessage rxMsg;
+ rxMsg.len = 6;
+ can.read(rxMsg); // read message into Rx message storage
+ int id = rxMsg.data[0];
+ for (int i = 0; i< N_MOTORS; i++)
+ {
+ if(motors[i].control.id == id)
+ {
+ memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
+ unpack_reply(&motors[i]);
+ }
+ }
+}
+void init_motors(int ids[N_MOTORS])
+/* Initialize buffer lengths and IDs of the motors in the list */
+{
+ for(int i = 0; i<N_MOTORS; i++)
+ {
+ motors[i].txMsg.len = 8;
+ motors[i].rxMsg.len = 6;
+ motors[i].control.id = ids[i];
+ motors[i].txMsg.id = ids[i];
+ motors[i].control.p_des = 0;
+ motors[i].control.v_des = 0;
+ motors[i].control.kp = 0;
+ motors[i].control.kd = 0;
+ motors[i].control.i_ff = 0;
+ }
+}