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@1:d24fd64d1fcb, 2019-06-30 (annotated)

Committer:
benkatz
Date:
Sun Jun 30 02:04:46 2019 +0000
Revision:
1:d24fd64d1fcb
Parent:
0:d6186b8990c5
Child:
2:36a254d3dbf3
updating

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 0:d6186b8990c5 2 #define CAN_ID 0x1
benkatz 0:d6186b8990c5 3
benkatz 0:d6186b8990c5 4 #include "mbed.h"
benkatz 0:d6186b8990c5 5 #include "math_ops.h"
benkatz 0:d6186b8990c5 6
benkatz 0:d6186b8990c5 7
benkatz 0:d6186b8990c5 8 Serial pc(PA_2, PA_3);
benkatz 1:d24fd64d1fcb 9 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 0:d6186b8990c5 10 CANMessage rxMsg;
benkatz 0:d6186b8990c5 11 CANMessage txMsg1;
benkatz 0:d6186b8990c5 12 CANMessage txMsg2;
benkatz 0:d6186b8990c5 13 int ledState;
benkatz 0:d6186b8990c5 14 Timer timer;
benkatz 0:d6186b8990c5 15 Ticker sendCAN;
benkatz 0:d6186b8990c5 16 int counter = 0;
benkatz 0:d6186b8990c5 17 volatile bool msgAvailable = false;
benkatz 0:d6186b8990c5 18 Ticker loop;
benkatz 0:d6186b8990c5 19
benkatz 0:d6186b8990c5 20 float theta1, theta2, dtheta1, dtheta2;
benkatz 0:d6186b8990c5 21
benkatz 0:d6186b8990c5 22 /// Value Limits ///
benkatz 1:d24fd64d1fcb 23 #define P_MIN -12.5f
benkatz 0:d6186b8990c5 24 #define P_MAX 12.5f
benkatz 1:d24fd64d1fcb 25 #define V_MIN -45.0f
benkatz 1:d24fd64d1fcb 26 #define V_MAX 45.0f
benkatz 0:d6186b8990c5 27 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 28 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 29 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 30 #define KD_MAX 5.0f
benkatz 0:d6186b8990c5 31 #define T_MIN -18.0f
benkatz 0:d6186b8990c5 32 #define T_MAX 18.0f
benkatz 0:d6186b8990c5 33
benkatz 0:d6186b8990c5 34 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 35 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 36 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 37 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 38 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 39 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 40 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 41 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 42 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 43 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 44 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 45 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 46 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 47 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 48 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 49 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 50
benkatz 0:d6186b8990c5 51 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){
benkatz 0:d6186b8990c5 52 /// limit data to be within bounds ///
benkatz 0:d6186b8990c5 53 p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
benkatz 0:d6186b8990c5 54 v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
benkatz 0:d6186b8990c5 55 kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
benkatz 0:d6186b8990c5 56 kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
benkatz 0:d6186b8990c5 57 t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
benkatz 0:d6186b8990c5 58 /// convert floats to unsigned ints ///
benkatz 0:d6186b8990c5 59 int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 60 int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 61 int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 0:d6186b8990c5 62 int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 0:d6186b8990c5 63 int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
benkatz 0:d6186b8990c5 64 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 65 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 66 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 67 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 68 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 69 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 70 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 71 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 72 msg->data[7] = t_int&0xff;
benkatz 0:d6186b8990c5 73 }
benkatz 0:d6186b8990c5 74
benkatz 0:d6186b8990c5 75 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 76 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 77 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 78 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 79 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 80 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 81 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 82 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 83 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 84 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 85 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 86
benkatz 0:d6186b8990c5 87 void unpack_reply(CANMessage msg){
benkatz 0:d6186b8990c5 88 /// unpack ints from can buffer ///
benkatz 0:d6186b8990c5 89 int id = msg.data[0];
benkatz 0:d6186b8990c5 90 int p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 0:d6186b8990c5 91 int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 0:d6186b8990c5 92 int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 0:d6186b8990c5 93 /// convert ints to floats ///
benkatz 0:d6186b8990c5 94 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 95 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 96 float i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
benkatz 0:d6186b8990c5 97
benkatz 0:d6186b8990c5 98 if(id == 2){
benkatz 0:d6186b8990c5 99 theta1 = p;
benkatz 0:d6186b8990c5 100 dtheta1 = v;
benkatz 0:d6186b8990c5 101 }
benkatz 0:d6186b8990c5 102 else if(id ==3){
benkatz 0:d6186b8990c5 103 theta2 = p;
benkatz 0:d6186b8990c5 104 dtheta2 = v;
benkatz 0:d6186b8990c5 105 }
benkatz 0:d6186b8990c5 106
benkatz 1:d24fd64d1fcb 107 //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i);
benkatz 0:d6186b8990c5 108 }
benkatz 0:d6186b8990c5 109
benkatz 0:d6186b8990c5 110 void onMsgReceived() {
benkatz 0:d6186b8990c5 111 can.read(rxMsg); // read message into Rx message storage
benkatz 0:d6186b8990c5 112 unpack_reply(rxMsg);
benkatz 0:d6186b8990c5 113 }
benkatz 0:d6186b8990c5 114
benkatz 0:d6186b8990c5 115
benkatz 0:d6186b8990c5 116 void sendCMD(){
benkatz 0:d6186b8990c5 117 /// bilateral teleoperation demo ///
benkatz 1:d24fd64d1fcb 118 pack_cmd(&txMsg1, 0, 0, 0, 0, -0.1f);
benkatz 1:d24fd64d1fcb 119 //pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
benkatz 1:d24fd64d1fcb 120 //pack_cmd(&txMsg2, 0, 0, 0, 0, 1.0);
benkatz 1:d24fd64d1fcb 121 //pack_cmd(&txMsg1, 0, 0, 0, 0, 1.0);
benkatz 1:d24fd64d1fcb 122 //can.write(txMsg2);
benkatz 0:d6186b8990c5 123 wait(.0003); // Give motor 1 time to respond.
benkatz 0:d6186b8990c5 124 can.write(txMsg1);
benkatz 0:d6186b8990c5 125 }
benkatz 0:d6186b8990c5 126
benkatz 0:d6186b8990c5 127 void serial_isr(){
benkatz 0:d6186b8990c5 128 /// hangle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 129 while(pc.readable()){
benkatz 0:d6186b8990c5 130 char c = pc.getc();
benkatz 0:d6186b8990c5 131 switch(c){
benkatz 0:d6186b8990c5 132 case(27):
benkatz 0:d6186b8990c5 133 printf("\n\r exiting motor mode \n\r");
benkatz 0:d6186b8990c5 134 txMsg1.data[0] = 0xFF;
benkatz 0:d6186b8990c5 135 txMsg1.data[1] = 0xFF;
benkatz 0:d6186b8990c5 136 txMsg1.data[2] = 0xFF;
benkatz 0:d6186b8990c5 137 txMsg1.data[3] = 0xFF;
benkatz 0:d6186b8990c5 138 txMsg1.data[4] = 0xFF;
benkatz 0:d6186b8990c5 139 txMsg1.data[5] = 0xFF;
benkatz 0:d6186b8990c5 140 txMsg1.data[6] = 0xFF;
benkatz 0:d6186b8990c5 141 txMsg1.data[7] = 0xFD;
benkatz 0:d6186b8990c5 142
benkatz 0:d6186b8990c5 143 txMsg2.data[0] = 0xFF;
benkatz 0:d6186b8990c5 144 txMsg2.data[1] = 0xFF;
benkatz 0:d6186b8990c5 145 txMsg2.data[2] = 0xFF;
benkatz 0:d6186b8990c5 146 txMsg2.data[3] = 0xFF;
benkatz 0:d6186b8990c5 147 txMsg2.data[4] = 0xFF;
benkatz 0:d6186b8990c5 148 txMsg2.data[5] = 0xFF;
benkatz 0:d6186b8990c5 149 txMsg2.data[6] = 0xFF;
benkatz 0:d6186b8990c5 150 txMsg2.data[7] = 0xFD;
benkatz 0:d6186b8990c5 151 break;
benkatz 0:d6186b8990c5 152 case('m'):
benkatz 0:d6186b8990c5 153 printf("\n\r entering motor mode \n\r");
benkatz 0:d6186b8990c5 154 txMsg1.data[0] = 0xFF;
benkatz 0:d6186b8990c5 155 txMsg1.data[1] = 0xFF;
benkatz 0:d6186b8990c5 156 txMsg1.data[2] = 0xFF;
benkatz 0:d6186b8990c5 157 txMsg1.data[3] = 0xFF;
benkatz 0:d6186b8990c5 158 txMsg1.data[4] = 0xFF;
benkatz 0:d6186b8990c5 159 txMsg1.data[5] = 0xFF;
benkatz 0:d6186b8990c5 160 txMsg1.data[6] = 0xFF;
benkatz 0:d6186b8990c5 161 txMsg1.data[7] = 0xFC;
benkatz 0:d6186b8990c5 162
benkatz 0:d6186b8990c5 163 txMsg2.data[0] = 0xFF;
benkatz 0:d6186b8990c5 164 txMsg2.data[1] = 0xFF;
benkatz 0:d6186b8990c5 165 txMsg2.data[2] = 0xFF;
benkatz 0:d6186b8990c5 166 txMsg2.data[3] = 0xFF;
benkatz 0:d6186b8990c5 167 txMsg2.data[4] = 0xFF;
benkatz 0:d6186b8990c5 168 txMsg2.data[5] = 0xFF;
benkatz 0:d6186b8990c5 169 txMsg2.data[6] = 0xFF;
benkatz 0:d6186b8990c5 170 txMsg2.data[7] = 0xFC;
benkatz 0:d6186b8990c5 171 break;
benkatz 0:d6186b8990c5 172 case('z'):
benkatz 0:d6186b8990c5 173 printf("\n\r zeroing \n\r");
benkatz 0:d6186b8990c5 174 txMsg1.data[0] = 0xFF;
benkatz 0:d6186b8990c5 175 txMsg1.data[1] = 0xFF;
benkatz 0:d6186b8990c5 176 txMsg1.data[2] = 0xFF;
benkatz 0:d6186b8990c5 177 txMsg1.data[3] = 0xFF;
benkatz 0:d6186b8990c5 178 txMsg1.data[4] = 0xFF;
benkatz 0:d6186b8990c5 179 txMsg1.data[5] = 0xFF;
benkatz 0:d6186b8990c5 180 txMsg1.data[6] = 0xFF;
benkatz 0:d6186b8990c5 181 txMsg1.data[7] = 0xFE;
benkatz 0:d6186b8990c5 182
benkatz 0:d6186b8990c5 183 txMsg2.data[0] = 0xFF;
benkatz 0:d6186b8990c5 184 txMsg2.data[1] = 0xFF;
benkatz 0:d6186b8990c5 185 txMsg2.data[2] = 0xFF;
benkatz 0:d6186b8990c5 186 txMsg2.data[3] = 0xFF;
benkatz 0:d6186b8990c5 187 txMsg2.data[4] = 0xFF;
benkatz 0:d6186b8990c5 188 txMsg2.data[5] = 0xFF;
benkatz 0:d6186b8990c5 189 txMsg2.data[6] = 0xFF;
benkatz 0:d6186b8990c5 190 txMsg2.data[7] = 0xFE;
benkatz 0:d6186b8990c5 191 break;
benkatz 0:d6186b8990c5 192 }
benkatz 0:d6186b8990c5 193 }
benkatz 0:d6186b8990c5 194 can.write(txMsg1);
benkatz 0:d6186b8990c5 195 can.write(txMsg2);
benkatz 0:d6186b8990c5 196
benkatz 0:d6186b8990c5 197 }
benkatz 0:d6186b8990c5 198
benkatz 0:d6186b8990c5 199 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
benkatz 0:d6186b8990c5 200 int main() {
benkatz 0:d6186b8990c5 201 pc.baud(921600);
benkatz 0:d6186b8990c5 202 pc.attach(&serial_isr);
benkatz 0:d6186b8990c5 203 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 0:d6186b8990c5 204 can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 0:d6186b8990c5 205 printf("Master\n\r");
benkatz 0:d6186b8990c5 206 //printf("%d\n\r", RX_ID << 18);
benkatz 0:d6186b8990c5 207 int count = 0;
benkatz 0:d6186b8990c5 208 txMsg1.len = 8; //transmit 8 bytes
benkatz 0:d6186b8990c5 209 txMsg2.len = 8; //transmit 8 bytes
benkatz 0:d6186b8990c5 210 rxMsg.len = 6; //receive 5 bytes
benkatz 0:d6186b8990c5 211 loop.attach(&sendCMD, .001);
benkatz 0:d6186b8990c5 212 txMsg1.id = 0x2; //1st motor ID
benkatz 0:d6186b8990c5 213 txMsg2.id = 0x3; //2nd motor ID
benkatz 0:d6186b8990c5 214 pack_cmd(&txMsg1, 0, 0, 0, 0, 0); //Start out by sending all 0's
benkatz 0:d6186b8990c5 215 pack_cmd(&txMsg2, 0, 0, 0, 0, 0);
benkatz 0:d6186b8990c5 216 timer.start();
benkatz 0:d6186b8990c5 217
benkatz 0:d6186b8990c5 218 while(1) {
benkatz 0:d6186b8990c5 219
benkatz 0:d6186b8990c5 220 }
benkatz 0:d6186b8990c5 221
benkatz 0:d6186b8990c5 222 }
benkatz 0:d6186b8990c5 223
benkatz 0:d6186b8990c5 224
benkatz 0:d6186b8990c5 225
benkatz 0:d6186b8990c5 226
benkatz 0:d6186b8990c5 227