Ben Katz
/
MotorModuleExample
MotorModule CAN example
main.cpp
- Committer:
- benkatz
- Date:
- 2019-08-08
- Revision:
- 3:f0d054d896f9
- Parent:
- 2:36a254d3dbf3
- Child:
- 4:0ce97b9fde37
File content as of revision 3:f0d054d896f9:
#define CAN_ID 0x0 #include "mbed.h" #include "math_ops.h" #include "MotorModule.h" 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 Ticker loop; // Control loop interrupt handler int loop_counter = 0; #define DT .01 // Control loop period #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 control() { /* Your control loop goes here. */ /* Update torques, position/velocity setpoints, etc */ motors[0].control.p_des = 10.0f*sin(.01f*loop_counter); motors[0].control.kd = .5f; motors[0].control.kp = 1.0f; /* */ for(int i = 0; i<N_MOTORS; i++) { pack_cmd(&motors[i]); can.write(motors[i].txMsg); } printf("%f\n\r", motors[0].state.position); // This will print to the computer. Usefull for debugging loop_counter++; // Increment loop counter } 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 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. } } /* 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; } }