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@5:a2e3d0213315, 2019-12-09 (annotated)

Committer:
kfmurph2
Date:
Mon Dec 09 23:41:16 2019 +0000
Revision:
5:a2e3d0213315
Parent:
4:0ce97b9fde37
Child:
6:95dc3761f64a
First run of examples.

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 4:0ce97b9fde37 15 #define DT .01f // 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 4:0ce97b9fde37 25
benkatz 2:36a254d3dbf3 26 void control()
benkatz 2:36a254d3dbf3 27 {
kfmurph2 5:a2e3d0213315 28 //set the other values to see if it keeps the motor from freaking out
kfmurph2 5:a2e3d0213315 29 motors[0].control.kp = 1;
kfmurph2 5:a2e3d0213315 30 motors[1].control.kp = 1;
benkatz 2:36a254d3dbf3 31 /* Your control loop goes here. */
benkatz 2:36a254d3dbf3 32 /* Update torques, position/velocity setpoints, etc */
benkatz 4:0ce97b9fde37 33 //motors[0].control.p_des = -40 + 40.0f*cos(.01f*loop_counter);
benkatz 4:0ce97b9fde37 34
kfmurph2 5:a2e3d0213315 35 float tilt_angle = 13.5f;
benkatz 4:0ce97b9fde37 36
benkatz 4:0ce97b9fde37 37 float t = DT*loop_counter;
benkatz 4:0ce97b9fde37 38 if(t<1)
benkatz 4:0ce97b9fde37 39 {
benkatz 4:0ce97b9fde37 40 motors[1].control.p_des = -tilt_angle*t; // dump left to -1.5
benkatz 4:0ce97b9fde37 41 motors[0].control.p_des = 0;
benkatz 4:0ce97b9fde37 42 }
benkatz 4:0ce97b9fde37 43 else if(t>1 && t<3)
benkatz 4:0ce97b9fde37 44 {
benkatz 4:0ce97b9fde37 45 motors[1].control.p_des = tilt_angle*(t-1.0f) - tilt_angle; // dump right from -1.5 to 1.5
benkatz 4:0ce97b9fde37 46 motors[0].control.p_des = 0;
benkatz 4:0ce97b9fde37 47 }
benkatz 4:0ce97b9fde37 48 else if(t>3 && t<4)
benkatz 4:0ce97b9fde37 49 {
benkatz 4:0ce97b9fde37 50 motors[1].control.p_des = -tilt_angle*(t-3.0f) + tilt_angle;
benkatz 4:0ce97b9fde37 51 motors[0].control.p_des = 0;
benkatz 4:0ce97b9fde37 52 } // center from 1.5 to 0
benkatz 4:0ce97b9fde37 53 else if (t>4 && t<7)
benkatz 4:0ce97b9fde37 54 {
benkatz 4:0ce97b9fde37 55 motors[1].control.p_des = 0;
benkatz 4:0ce97b9fde37 56 motors[0].control.p_des = -40.0f+40.0f*cos((t-4.0f));
benkatz 4:0ce97b9fde37 57 }
benkatz 4:0ce97b9fde37 58 else if(t>7 && t<8)
benkatz 4:0ce97b9fde37 59 {
benkatz 4:0ce97b9fde37 60 motors[1].control.p_des = -tilt_angle*(t-7.0f); // dump left to -1.5
benkatz 4:0ce97b9fde37 61 motors[0].control.p_des = -80;
benkatz 4:0ce97b9fde37 62 }
benkatz 4:0ce97b9fde37 63 else if(t>8 && t<10)
benkatz 4:0ce97b9fde37 64 {
benkatz 4:0ce97b9fde37 65 motors[1].control.p_des = tilt_angle*(t-8.0f) - tilt_angle; // dump right from -1.5 to 1.5
benkatz 4:0ce97b9fde37 66 motors[0].control.p_des = -80;
benkatz 4:0ce97b9fde37 67 }
benkatz 4:0ce97b9fde37 68 else if(t>10 && t<11)
benkatz 4:0ce97b9fde37 69 {
benkatz 4:0ce97b9fde37 70 motors[1].control.p_des = -tilt_angle*(t-10.0f) + tilt_angle;
benkatz 4:0ce97b9fde37 71 motors[0].control.p_des = -80;
benkatz 4:0ce97b9fde37 72 } // center from 1.5 to 0
benkatz 4:0ce97b9fde37 73 else if (t>11 && t<14)
benkatz 4:0ce97b9fde37 74 {
benkatz 4:0ce97b9fde37 75 motors[1].control.p_des = 0;
benkatz 4:0ce97b9fde37 76 motors[0].control.p_des = -40.0f-40.0f*cos((t-11.0f));
benkatz 4:0ce97b9fde37 77 }
benkatz 4:0ce97b9fde37 78
benkatz 2:36a254d3dbf3 79 motors[0].control.kd = .5f;
benkatz 4:0ce97b9fde37 80 motors[0].control.kp = 2.0f;
benkatz 4:0ce97b9fde37 81
kfmurph2 5:a2e3d0213315 82 motors[1].control.p_des = 2*sin(.01f*loop_counter);
benkatz 4:0ce97b9fde37 83 motors[1].control.kd = 1.0f;
benkatz 4:0ce97b9fde37 84 motors[1].control.kp = 20.0f;
benkatz 2:36a254d3dbf3 85 /* */
benkatz 0:d6186b8990c5 86
benkatz 4:0ce97b9fde37 87 if(t>14){loop_counter = 0;}
benkatz 4:0ce97b9fde37 88
benkatz 2:36a254d3dbf3 89 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 90 {
benkatz 2:36a254d3dbf3 91 pack_cmd(&motors[i]);
benkatz 2:36a254d3dbf3 92 can.write(motors[i].txMsg);
benkatz 2:36a254d3dbf3 93 }
benkatz 0:d6186b8990c5 94
benkatz 0:d6186b8990c5 95
benkatz 4:0ce97b9fde37 96 //printf("%f %f\n\r", motors[0].control.p_des, motors[1].control.p_des); // This will print to the computer. Usefull for debugging
kfmurph2 5:a2e3d0213315 97 // printf("%f %f\n\r", motors[0].state.position, motors[1].state.position);
kfmurph2 5:a2e3d0213315 98 printf("1 \t pos1: %f \t des1: %f \t pos2: %f \t des2: %f\n\r", motors[0].state.position, motors[0].control.p_des, motors[1].state.position, motors[1].control.p_des);
benkatz 2:36a254d3dbf3 99 loop_counter++; // Increment loop counter
benkatz 0:d6186b8990c5 100 }
benkatz 0:d6186b8990c5 101
benkatz 0:d6186b8990c5 102
benkatz 2:36a254d3dbf3 103 int main()
benkatz 2:36a254d3dbf3 104 {
benkatz 2:36a254d3dbf3 105
benkatz 2:36a254d3dbf3 106 pc.baud(921600); // Set baud rate for communication over USB serial
benkatz 2:36a254d3dbf3 107 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 2:36a254d3dbf3 108 can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
benkatz 2:36a254d3dbf3 109
kfmurph2 5:a2e3d0213315 110 int ids[N_MOTORS] = {1,2}; // List of motor CAN ID's
benkatz 2:36a254d3dbf3 111 init_motors(ids); // Initialize the list of motors
benkatz 0:d6186b8990c5 112
kfmurph2 5:a2e3d0213315 113 enable_motor(&motors[0], &can); // Enable motors
benkatz 4:0ce97b9fde37 114 enable_motor(&motors[1], &can);
kfmurph2 5:a2e3d0213315 115
kfmurph2 5:a2e3d0213315 116 zero_motor(&motors[0], &can); //Zero motors
kfmurph2 5:a2e3d0213315 117 zero_motor(&motors[1], &can);
benkatz 4:0ce97b9fde37 118 wait(1); // Wait 1 second
kfmurph2 5:a2e3d0213315 119
benkatz 3:f0d054d896f9 120 //disable_motor(&motors[0], &can); // Disable first motor
benkatz 4:0ce97b9fde37 121 //disable_motor(&motors[1], &can);
kfmurph2 5:a2e3d0213315 122
benkatz 2:36a254d3dbf3 123 loop.attach(&control, DT); // Start running the contorl interrupt at 1/DT Hz
benkatz 0:d6186b8990c5 124
benkatz 2:36a254d3dbf3 125 while(1)
benkatz 2:36a254d3dbf3 126 {
benkatz 2:36a254d3dbf3 127 // Usuallly nothing should run here. Instead run control in the interrupt.
benkatz 0:d6186b8990c5 128 }
benkatz 0:d6186b8990c5 129
benkatz 2:36a254d3dbf3 130 }
benkatz 0:d6186b8990c5 131
benkatz 0:d6186b8990c5 132
benkatz 2:36a254d3dbf3 133 /* low-level communication functoins below. Do not touch */
benkatz 0:d6186b8990c5 134
benkatz 0:d6186b8990c5 135
benkatz 2:36a254d3dbf3 136 void onMsgReceived()
benkatz 2:36a254d3dbf3 137 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
benkatz 2:36a254d3dbf3 138 {
benkatz 2:36a254d3dbf3 139 CANMessage rxMsg;
benkatz 2:36a254d3dbf3 140 rxMsg.len = 6;
benkatz 2:36a254d3dbf3 141 can.read(rxMsg); // read message into Rx message storage
benkatz 2:36a254d3dbf3 142 int id = rxMsg.data[0];
benkatz 2:36a254d3dbf3 143 for (int i = 0; i< N_MOTORS; i++)
benkatz 2:36a254d3dbf3 144 {
benkatz 2:36a254d3dbf3 145 if(motors[i].control.id == id)
benkatz 2:36a254d3dbf3 146 {
benkatz 2:36a254d3dbf3 147 memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
benkatz 2:36a254d3dbf3 148 unpack_reply(&motors[i]);
benkatz 2:36a254d3dbf3 149 }
benkatz 2:36a254d3dbf3 150 }
benkatz 2:36a254d3dbf3 151 }
benkatz 0:d6186b8990c5 152
benkatz 2:36a254d3dbf3 153 void init_motors(int ids[N_MOTORS])
benkatz 2:36a254d3dbf3 154 /* Initialize buffer lengths and IDs of the motors in the list */
benkatz 2:36a254d3dbf3 155 {
benkatz 2:36a254d3dbf3 156 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 157 {
benkatz 2:36a254d3dbf3 158 motors[i].txMsg.len = 8;
benkatz 2:36a254d3dbf3 159 motors[i].rxMsg.len = 6;
benkatz 2:36a254d3dbf3 160 motors[i].control.id = ids[i];
benkatz 2:36a254d3dbf3 161 motors[i].txMsg.id = ids[i];
benkatz 2:36a254d3dbf3 162 motors[i].control.p_des = 0;
benkatz 2:36a254d3dbf3 163 motors[i].control.v_des = 0;
benkatz 2:36a254d3dbf3 164 motors[i].control.kp = 0;
benkatz 2:36a254d3dbf3 165 motors[i].control.kd = 0;
benkatz 2:36a254d3dbf3 166 motors[i].control.i_ff = 0;
benkatz 2:36a254d3dbf3 167 }
benkatz 2:36a254d3dbf3 168 }