Sim Youngwoo / Mbed 2 deprecated V3PowerCycle

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Motor_Feedforward

main.cpp@2:36a254d3dbf3, 2019-08-08 (annotated)

Committer:
benkatz
Date:
Thu Aug 08 17:03:30 2019 +0000
Revision:
2:36a254d3dbf3
Parent:
1:d24fd64d1fcb
Child:
3:f0d054d896f9
Working example

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 2:36a254d3dbf3 29 motors[0].control.v_des = 5.0f*sin(.01f*loop_counter);
benkatz 2:36a254d3dbf3 30 motors[0].control.kd = .5f;
benkatz 2:36a254d3dbf3 31 /* */
benkatz 0:d6186b8990c5 32
benkatz 2:36a254d3dbf3 33 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 34 {
benkatz 2:36a254d3dbf3 35 pack_cmd(&motors[i]);
benkatz 2:36a254d3dbf3 36 can.write(motors[i].txMsg);
benkatz 2:36a254d3dbf3 37 }
benkatz 0:d6186b8990c5 38
benkatz 0:d6186b8990c5 39
benkatz 2:36a254d3dbf3 40 printf("%f\n\r", motors[0].state.velocity); // This will print to the computer. Usefull for debugging
benkatz 2:36a254d3dbf3 41 loop_counter++; // Increment loop counter
benkatz 0:d6186b8990c5 42 }
benkatz 0:d6186b8990c5 43
benkatz 0:d6186b8990c5 44
benkatz 2:36a254d3dbf3 45 int main()
benkatz 2:36a254d3dbf3 46 {
benkatz 2:36a254d3dbf3 47
benkatz 2:36a254d3dbf3 48 pc.baud(921600); // Set baud rate for communication over USB serial
benkatz 2:36a254d3dbf3 49 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 2:36a254d3dbf3 50 can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
benkatz 2:36a254d3dbf3 51
benkatz 2:36a254d3dbf3 52 int ids[N_MOTORS] = {1, 2}; // List of motor CAN ID's
benkatz 2:36a254d3dbf3 53 init_motors(ids); // Initialize the list of motors
benkatz 0:d6186b8990c5 54
benkatz 2:36a254d3dbf3 55 enable_motor(&motors[0], &can); // Enable first motor
benkatz 2:36a254d3dbf3 56 wait(1); // Wait 1 second
benkatz 2:36a254d3dbf3 57 disable_motor(&motors[0], &can); // Disable first motor
benkatz 2:36a254d3dbf3 58
benkatz 2:36a254d3dbf3 59 loop.attach(&control, DT); // Start running the contorl interrupt at 1/DT Hz
benkatz 0:d6186b8990c5 60
benkatz 2:36a254d3dbf3 61 while(1)
benkatz 2:36a254d3dbf3 62 {
benkatz 2:36a254d3dbf3 63 // Usuallly nothing should run here. Instead run control in the interrupt.
benkatz 0:d6186b8990c5 64 }
benkatz 0:d6186b8990c5 65
benkatz 2:36a254d3dbf3 66 }
benkatz 0:d6186b8990c5 67
benkatz 0:d6186b8990c5 68
benkatz 2:36a254d3dbf3 69 /* low-level communication functoins below. Do not touch */
benkatz 0:d6186b8990c5 70
benkatz 0:d6186b8990c5 71
benkatz 2:36a254d3dbf3 72 void onMsgReceived()
benkatz 2:36a254d3dbf3 73 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
benkatz 2:36a254d3dbf3 74 {
benkatz 2:36a254d3dbf3 75 CANMessage rxMsg;
benkatz 2:36a254d3dbf3 76 rxMsg.len = 6;
benkatz 2:36a254d3dbf3 77 can.read(rxMsg); // read message into Rx message storage
benkatz 2:36a254d3dbf3 78 int id = rxMsg.data[0];
benkatz 2:36a254d3dbf3 79 for (int i = 0; i< N_MOTORS; i++)
benkatz 2:36a254d3dbf3 80 {
benkatz 2:36a254d3dbf3 81 if(motors[i].control.id == id)
benkatz 2:36a254d3dbf3 82 {
benkatz 2:36a254d3dbf3 83 memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
benkatz 2:36a254d3dbf3 84 unpack_reply(&motors[i]);
benkatz 2:36a254d3dbf3 85 }
benkatz 2:36a254d3dbf3 86 }
benkatz 2:36a254d3dbf3 87 }
benkatz 0:d6186b8990c5 88
benkatz 2:36a254d3dbf3 89 void init_motors(int ids[N_MOTORS])
benkatz 2:36a254d3dbf3 90 /* Initialize buffer lengths and IDs of the motors in the list */
benkatz 2:36a254d3dbf3 91 {
benkatz 2:36a254d3dbf3 92 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 93 {
benkatz 2:36a254d3dbf3 94 motors[i].txMsg.len = 8;
benkatz 2:36a254d3dbf3 95 motors[i].rxMsg.len = 6;
benkatz 2:36a254d3dbf3 96 motors[i].control.id = ids[i];
benkatz 2:36a254d3dbf3 97 motors[i].txMsg.id = ids[i];
benkatz 2:36a254d3dbf3 98 motors[i].control.p_des = 0;
benkatz 2:36a254d3dbf3 99 motors[i].control.v_des = 0;
benkatz 2:36a254d3dbf3 100 motors[i].control.kp = 0;
benkatz 2:36a254d3dbf3 101 motors[i].control.kd = 0;
benkatz 2:36a254d3dbf3 102 motors[i].control.i_ff = 0;
benkatz 2:36a254d3dbf3 103 }
benkatz 2:36a254d3dbf3 104 }