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@10:be0f4cf7bd8d, 2020-09-23 (annotated)

Committer:
ywsim
Date:
Wed Sep 23 18:37:08 2020 +0000
Branch:
yw_sim_devel
Revision:
10:be0f4cf7bd8d
Parent:
9:3e15c7d42813 
Test Separate Repo

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