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@7:e4fc29c6f014, 2020-09-19 (annotated)

Committer:
ywsim
Date:
Sat Sep 19 01:51:56 2020 +0000
Revision:
7:e4fc29c6f014
Parent:
6:95dc3761f64a
Child:
9:3e15c7d42813
First batch,  you need to change ID, and IDnum;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 2:36a254d3dbf3 1 #define CAN_ID 0x0
benkatz 0:d6186b8990c5 2 #include "mbed.h"
benkatz 0:d6186b8990c5 3 #include "math_ops.h"
benkatz 2:36a254d3dbf3 4 #include "MotorModule.h"
benkatz 2:36a254d3dbf3 5
benkatz 0:d6186b8990c5 6
benkatz 0:d6186b8990c5 7
benkatz 2:36a254d3dbf3 8 Serial pc(PA_2, PA_3); // Serial port to the computer
benkatz 1:d24fd64d1fcb 9 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 0:d6186b8990c5 10
ywsim 6:95dc3761f64a 11 Ticker loop_ctrl; // Control loop interrupt handler
ywsim 6:95dc3761f64a 12 Ticker loop_msg;
ywsim 6:95dc3761f64a 13
ywsim 6:95dc3761f64a 14 #define DT_ctrl .001f // Control loop period
ywsim 6:95dc3761f64a 15 #define DT_msg .01f // msg loop period
ywsim 6:95dc3761f64a 16 #define N_MOTORS 1 // Number of motors on the can bus
ywsim 6:95dc3761f64a 17
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 0:d6186b8990c5 23
ywsim 6:95dc3761f64a 24 int loop_counter = 0;
ywsim 6:95dc3761f64a 25 int newprint = 1;
ywsim 6:95dc3761f64a 26 int nskip = 5;
ywsim 6:95dc3761f64a 27 int skip_counter = 0;
benkatz 4:0ce97b9fde37 28
ywsim 6:95dc3761f64a 29 float t = 0.0;
ywsim 6:95dc3761f64a 30 float A= 0.5;
ywsim 6:95dc3761f64a 31 float f = 1.0;
ywsim 7:e4fc29c6f014 32 double mycur = 0.35;
ywsim 6:95dc3761f64a 33 bool flagCtrl = true;
ywsim 6:95dc3761f64a 34
ywsim 6:95dc3761f64a 35 void printnew(){
ywsim 6:95dc3761f64a 36 newprint = 1;
benkatz 0:d6186b8990c5 37 }
benkatz 0:d6186b8990c5 38
ywsim 6:95dc3761f64a 39 void control(){
ywsim 6:95dc3761f64a 40 t = DT_ctrl*loop_counter; //time in seconds
ywsim 6:95dc3761f64a 41 if (flagCtrl) {
ywsim 6:95dc3761f64a 42 // if control is ON
ywsim 6:95dc3761f64a 43 if (t<1) {
ywsim 6:95dc3761f64a 44 motors[0].control.p_des = 0;
ywsim 6:95dc3761f64a 45 motors[0].control.v_des = 0;
ywsim 6:95dc3761f64a 46 motors[0].control.kp = 0;
ywsim 6:95dc3761f64a 47 motors[0].control.kd = 0;
ywsim 6:95dc3761f64a 48 }
ywsim 6:95dc3761f64a 49 if (1<=t<20){
ywsim 6:95dc3761f64a 50 motors[0].control.i_ff = mycur;
ywsim 7:e4fc29c6f014 51 motors[0].control.p_des = 0;
ywsim 6:95dc3761f64a 52 motors[0].control.kd = 0;
ywsim 6:95dc3761f64a 53 motors[0].control.kp = 0;
ywsim 6:95dc3761f64a 54 }
ywsim 6:95dc3761f64a 55 for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
ywsim 6:95dc3761f64a 56 pack_cmd(&motors[i]);
ywsim 6:95dc3761f64a 57 can.write(motors[i].txMsg);
ywsim 6:95dc3761f64a 58 }
ywsim 6:95dc3761f64a 59 } else {
ywsim 6:95dc3761f64a 60 // if control is OFF
ywsim 6:95dc3761f64a 61 motors[0].control.i_ff = 0.0f; //all set to zero (no control)
ywsim 6:95dc3761f64a 62 motors[0].control.kp = 0.0f;
ywsim 6:95dc3761f64a 63 motors[0].control.kd = 0.0f;
ywsim 6:95dc3761f64a 64
ywsim 6:95dc3761f64a 65 loop_counter = 0.0f; //re-zero loop timing
ywsim 6:95dc3761f64a 66
ywsim 6:95dc3761f64a 67 for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
ywsim 6:95dc3761f64a 68 pack_cmd(&motors[i]);
ywsim 6:95dc3761f64a 69 can.write(motors[i].txMsg);
ywsim 6:95dc3761f64a 70 }
ywsim 6:95dc3761f64a 71 }
ywsim 6:95dc3761f64a 72 loop_counter++; // increase loop counter
ywsim 6:95dc3761f64a 73 }
benkatz 0:d6186b8990c5 74
benkatz 2:36a254d3dbf3 75 int main()
benkatz 2:36a254d3dbf3 76 {
ywsim 6:95dc3761f64a 77 /* Setup & Initialization */
benkatz 2:36a254d3dbf3 78 pc.baud(921600); // Set baud rate for communication over USB serial
benkatz 2:36a254d3dbf3 79 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 2:36a254d3dbf3 80 can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
ywsim 6:95dc3761f64a 81
benkatz 2:36a254d3dbf3 82
ywsim 6:95dc3761f64a 83 int ids[N_MOTORS] = {1}; // List of motor CAN ID's
benkatz 2:36a254d3dbf3 84 init_motors(ids); // Initialize the list of motors
benkatz 0:d6186b8990c5 85
kfmurph2 5:a2e3d0213315 86 enable_motor(&motors[0], &can); // Enable motors
ywsim 6:95dc3761f64a 87 //enable_motor(&motors[1], &can);
kfmurph2 5:a2e3d0213315 88
kfmurph2 5:a2e3d0213315 89 zero_motor(&motors[0], &can); //Zero motors
ywsim 6:95dc3761f64a 90 //zero_motor(&motors[1], &can);
benkatz 4:0ce97b9fde37 91 wait(1); // Wait 1 second
kfmurph2 5:a2e3d0213315 92
benkatz 3:f0d054d896f9 93 //disable_motor(&motors[0], &can); // Disable first motor
benkatz 4:0ce97b9fde37 94 //disable_motor(&motors[1], &can);
kfmurph2 5:a2e3d0213315 95
ywsim 6:95dc3761f64a 96 /* Interrupt Enables */
ywsim 6:95dc3761f64a 97 loop_ctrl.attach(&control, DT_ctrl); // Start running the contorl interrupt at 1/DT Hz
ywsim 6:95dc3761f64a 98 loop_msg.attach(&printnew, DT_msg);
ywsim 6:95dc3761f64a 99
ywsim 6:95dc3761f64a 100 while(1) {
ywsim 6:95dc3761f64a 101 if (pc.readable()) {
ywsim 6:95dc3761f64a 102 char c = pc.getc();
ywsim 6:95dc3761f64a 103 if(c == 's') {
ywsim 6:95dc3761f64a 104 flagCtrl = false;
ywsim 6:95dc3761f64a 105 printf("s");
ywsim 6:95dc3761f64a 106 }
ywsim 6:95dc3761f64a 107 if(c == 'g') {
ywsim 6:95dc3761f64a 108 flagCtrl = true;
ywsim 6:95dc3761f64a 109 printf("g");
ywsim 6:95dc3761f64a 110 }
ywsim 6:95dc3761f64a 111 if(c == 'e') {
ywsim 6:95dc3761f64a 112 mycur = mycur +0.1;
ywsim 6:95dc3761f64a 113 }
ywsim 6:95dc3761f64a 114 if(c == 'r') {
ywsim 6:95dc3761f64a 115 mycur = mycur - 0.1;
ywsim 6:95dc3761f64a 116 }
ywsim 6:95dc3761f64a 117
ywsim 6:95dc3761f64a 118 }
ywsim 6:95dc3761f64a 119 if(newprint == 1){
ywsim 6:95dc3761f64a 120 printf("%f \t%f \t%f \r\n", t, motors[0].state.position, motors[0].state.current);
ywsim 6:95dc3761f64a 121 //pc.putc(motors[0].state.position);
ywsim 6:95dc3761f64a 122 //pc.write(&motors[0].state.position);
ywsim 6:95dc3761f64a 123 //dat1.f = motors[0].state.position;
ywsim 6:95dc3761f64a 124 //pc.printf("%c%c%c%c", dat1.s[0], dat1.s[1], dat1.s[2], dat1.s[3]);
ywsim 6:95dc3761f64a 125 newprint = 0;
ywsim 6:95dc3761f64a 126 }
benkatz 0:d6186b8990c5 127 }
benkatz 2:36a254d3dbf3 128 }
benkatz 0:d6186b8990c5 129
benkatz 2:36a254d3dbf3 130 /* low-level communication functoins below. Do not touch */
benkatz 0:d6186b8990c5 131
benkatz 2:36a254d3dbf3 132 void onMsgReceived()
benkatz 2:36a254d3dbf3 133 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
benkatz 2:36a254d3dbf3 134 {
benkatz 2:36a254d3dbf3 135 CANMessage rxMsg;
benkatz 2:36a254d3dbf3 136 rxMsg.len = 6;
benkatz 2:36a254d3dbf3 137 can.read(rxMsg); // read message into Rx message storage
benkatz 2:36a254d3dbf3 138 int id = rxMsg.data[0];
benkatz 2:36a254d3dbf3 139 for (int i = 0; i< N_MOTORS; i++)
benkatz 2:36a254d3dbf3 140 {
benkatz 2:36a254d3dbf3 141 if(motors[i].control.id == id)
benkatz 2:36a254d3dbf3 142 {
benkatz 2:36a254d3dbf3 143 memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
benkatz 2:36a254d3dbf3 144 unpack_reply(&motors[i]);
benkatz 2:36a254d3dbf3 145 }
benkatz 2:36a254d3dbf3 146 }
benkatz 2:36a254d3dbf3 147 }
benkatz 0:d6186b8990c5 148
benkatz 2:36a254d3dbf3 149 void init_motors(int ids[N_MOTORS])
benkatz 2:36a254d3dbf3 150 /* Initialize buffer lengths and IDs of the motors in the list */
benkatz 2:36a254d3dbf3 151 {
benkatz 2:36a254d3dbf3 152 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 153 {
benkatz 2:36a254d3dbf3 154 motors[i].txMsg.len = 8;
benkatz 2:36a254d3dbf3 155 motors[i].rxMsg.len = 6;
benkatz 2:36a254d3dbf3 156 motors[i].control.id = ids[i];
benkatz 2:36a254d3dbf3 157 motors[i].txMsg.id = ids[i];
benkatz 2:36a254d3dbf3 158 motors[i].control.p_des = 0;
benkatz 2:36a254d3dbf3 159 motors[i].control.v_des = 0;
benkatz 2:36a254d3dbf3 160 motors[i].control.kp = 0;
benkatz 2:36a254d3dbf3 161 motors[i].control.kd = 0;
benkatz 2:36a254d3dbf3 162 motors[i].control.i_ff = 0;
benkatz 2:36a254d3dbf3 163 }
benkatz 2:36a254d3dbf3 164 }