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@6:95dc3761f64a, 2020-02-19 (annotated)

Committer:
ywsim
Date:
Wed Feb 19 04:52:01 2020 +0000
Revision:
6:95dc3761f64a
Parent:
5:a2e3d0213315
Child:
7:e4fc29c6f014
for marty;

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