Ben Katz / Mbed 2 deprecated MotorModuleExample

MotorModule CAN example

Dependencies:   mbed

main.cpp@3:f0d054d896f9, 2019-08-08 (annotated)

Committer:
benkatz
Date:
Thu Aug 08 17:39:17 2019 +0000
Revision:
3:f0d054d896f9
Parent:
2:36a254d3dbf3
Child:
4:0ce97b9fde37
updated position for +/- 15 turns

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 2:36a254d3dbf3 2 #define CAN_ID 0x0
benkatz 0:d6186b8990c5 3
benkatz 0:d6186b8990c5 4 #include "mbed.h"
benkatz 0:d6186b8990c5 5 #include "math_ops.h"
benkatz 2:36a254d3dbf3 6 #include "MotorModule.h"
benkatz 2:36a254d3dbf3 7
benkatz 0:d6186b8990c5 8
benkatz 0:d6186b8990c5 9
benkatz 2:36a254d3dbf3 10 Serial pc(PA_2, PA_3); // Serial port to the computer
benkatz 1:d24fd64d1fcb 11 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 0:d6186b8990c5 12
benkatz 2:36a254d3dbf3 13 Ticker loop; // Control loop interrupt handler
benkatz 2:36a254d3dbf3 14 int loop_counter = 0;
benkatz 2:36a254d3dbf3 15 #define DT .01 // Control loop period
benkatz 0:d6186b8990c5 16
benkatz 2:36a254d3dbf3 17 #define N_MOTORS 2 // Number of motors on the can bus
benkatz 2:36a254d3dbf3 18 MotorStruct motors[N_MOTORS]; // Create a list of the motors attached
benkatz 2:36a254d3dbf3 19
benkatz 2:36a254d3dbf3 20 /* Communication functions. Do not touch */
benkatz 2:36a254d3dbf3 21 void onMsgReceived();
benkatz 2:36a254d3dbf3 22 void init_motors(int ids[N_MOTORS]);
benkatz 2:36a254d3dbf3 23 /* */
benkatz 0:d6186b8990c5 24
benkatz 2:36a254d3dbf3 25 void control()
benkatz 2:36a254d3dbf3 26 {
benkatz 2:36a254d3dbf3 27 /* Your control loop goes here. */
benkatz 2:36a254d3dbf3 28 /* Update torques, position/velocity setpoints, etc */
benkatz 3:f0d054d896f9 29 motors[0].control.p_des = 10.0f*sin(.01f*loop_counter);
benkatz 2:36a254d3dbf3 30 motors[0].control.kd = .5f;
benkatz 3:f0d054d896f9 31 motors[0].control.kp = 1.0f;
benkatz 2:36a254d3dbf3 32 /* */
benkatz 0:d6186b8990c5 33
benkatz 2:36a254d3dbf3 34 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 35 {
benkatz 2:36a254d3dbf3 36 pack_cmd(&motors[i]);
benkatz 2:36a254d3dbf3 37 can.write(motors[i].txMsg);
benkatz 2:36a254d3dbf3 38 }
benkatz 0:d6186b8990c5 39
benkatz 0:d6186b8990c5 40
benkatz 3:f0d054d896f9 41 printf("%f\n\r", motors[0].state.position); // This will print to the computer. Usefull for debugging
benkatz 2:36a254d3dbf3 42 loop_counter++; // Increment loop counter
benkatz 0:d6186b8990c5 43 }
benkatz 0:d6186b8990c5 44
benkatz 0:d6186b8990c5 45
benkatz 2:36a254d3dbf3 46 int main()
benkatz 2:36a254d3dbf3 47 {
benkatz 2:36a254d3dbf3 48
benkatz 2:36a254d3dbf3 49 pc.baud(921600); // Set baud rate for communication over USB serial
benkatz 2:36a254d3dbf3 50 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 2:36a254d3dbf3 51 can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
benkatz 2:36a254d3dbf3 52
benkatz 2:36a254d3dbf3 53 int ids[N_MOTORS] = {1, 2}; // List of motor CAN ID's
benkatz 2:36a254d3dbf3 54 init_motors(ids); // Initialize the list of motors
benkatz 0:d6186b8990c5 55
benkatz 2:36a254d3dbf3 56 enable_motor(&motors[0], &can); // Enable first motor
benkatz 3:f0d054d896f9 57 //wait(1); // Wait 1 second
benkatz 3:f0d054d896f9 58 //disable_motor(&motors[0], &can); // Disable first motor
benkatz 2:36a254d3dbf3 59
benkatz 2:36a254d3dbf3 60 loop.attach(&control, DT); // Start running the contorl interrupt at 1/DT Hz
benkatz 0:d6186b8990c5 61
benkatz 2:36a254d3dbf3 62 while(1)
benkatz 2:36a254d3dbf3 63 {
benkatz 2:36a254d3dbf3 64 // Usuallly nothing should run here. Instead run control in the interrupt.
benkatz 0:d6186b8990c5 65 }
benkatz 0:d6186b8990c5 66
benkatz 2:36a254d3dbf3 67 }
benkatz 0:d6186b8990c5 68
benkatz 0:d6186b8990c5 69
benkatz 2:36a254d3dbf3 70 /* low-level communication functoins below. Do not touch */
benkatz 0:d6186b8990c5 71
benkatz 0:d6186b8990c5 72
benkatz 2:36a254d3dbf3 73 void onMsgReceived()
benkatz 2:36a254d3dbf3 74 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
benkatz 2:36a254d3dbf3 75 {
benkatz 2:36a254d3dbf3 76 CANMessage rxMsg;
benkatz 2:36a254d3dbf3 77 rxMsg.len = 6;
benkatz 2:36a254d3dbf3 78 can.read(rxMsg); // read message into Rx message storage
benkatz 2:36a254d3dbf3 79 int id = rxMsg.data[0];
benkatz 2:36a254d3dbf3 80 for (int i = 0; i< N_MOTORS; i++)
benkatz 2:36a254d3dbf3 81 {
benkatz 2:36a254d3dbf3 82 if(motors[i].control.id == id)
benkatz 2:36a254d3dbf3 83 {
benkatz 2:36a254d3dbf3 84 memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
benkatz 2:36a254d3dbf3 85 unpack_reply(&motors[i]);
benkatz 2:36a254d3dbf3 86 }
benkatz 2:36a254d3dbf3 87 }
benkatz 2:36a254d3dbf3 88 }
benkatz 0:d6186b8990c5 89
benkatz 2:36a254d3dbf3 90 void init_motors(int ids[N_MOTORS])
benkatz 2:36a254d3dbf3 91 /* Initialize buffer lengths and IDs of the motors in the list */
benkatz 2:36a254d3dbf3 92 {
benkatz 2:36a254d3dbf3 93 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 94 {
benkatz 2:36a254d3dbf3 95 motors[i].txMsg.len = 8;
benkatz 2:36a254d3dbf3 96 motors[i].rxMsg.len = 6;
benkatz 2:36a254d3dbf3 97 motors[i].control.id = ids[i];
benkatz 2:36a254d3dbf3 98 motors[i].txMsg.id = ids[i];
benkatz 2:36a254d3dbf3 99 motors[i].control.p_des = 0;
benkatz 2:36a254d3dbf3 100 motors[i].control.v_des = 0;
benkatz 2:36a254d3dbf3 101 motors[i].control.kp = 0;
benkatz 2:36a254d3dbf3 102 motors[i].control.kd = 0;
benkatz 2:36a254d3dbf3 103 motors[i].control.i_ff = 0;
benkatz 2:36a254d3dbf3 104 }
benkatz 2:36a254d3dbf3 105 }