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@4:0ce97b9fde37, 2019-08-08 (annotated)

Committer:
benkatz
Date:
Thu Aug 08 20:03:14 2019 +0000
Revision:
4:0ce97b9fde37
Parent:
3:f0d054d896f9
Child:
5:a2e3d0213315
test stand program;

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