1111
Dependencies: mbed-dev-f303 FastPWM3
main.cpp@51:adc72e125b15, 2020-08-06 (annotated)
- Committer:
- shaorui
- Date:
- Thu Aug 06 00:55:34 2020 +0000
- Revision:
- 51:adc72e125b15
- Parent:
- 50:1fe5c2c53af1
11111
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 22:60276ba87ac6 | 1 | /// high-bandwidth 3-phase motor control, for robots |
benkatz | 22:60276ba87ac6 | 2 | /// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others |
benkatz | 22:60276ba87ac6 | 3 | /// Hardware documentation can be found at build-its.blogspot.com |
benkatz | 22:60276ba87ac6 | 4 | /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling |
benkatz | 22:60276ba87ac6 | 5 | |
benkatz | 23:2adf23ee0305 | 6 | #define REST_MODE 0 |
benkatz | 23:2adf23ee0305 | 7 | #define CALIBRATION_MODE 1 |
benkatz | 26:2b865c00d7e9 | 8 | #define MOTOR_MODE 2 |
benkatz | 23:2adf23ee0305 | 9 | #define SETUP_MODE 4 |
benkatz | 23:2adf23ee0305 | 10 | #define ENCODER_MODE 5 |
Rushu | 49:7eac11914980 | 11 | #define TEST_TRAJECTORY_MODE 6 |
shaorui | 48:1b51771c3647 | 12 | #define J_CALIBRATION_MODE 7 |
benkatz | 44:efcde0af8390 | 13 | #define VERSION_NUM "1.6" |
benkatz | 26:2b865c00d7e9 | 14 | |
benkatz | 18:f1d56f4acb39 | 15 | |
benkatz | 26:2b865c00d7e9 | 16 | float __float_reg[64]; // Floats stored in flash |
shaorui | 48:1b51771c3647 | 17 | //int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table |
shaorui | 48:1b51771c3647 | 18 | int __int_reg[300]; |
shaorui | 48:1b51771c3647 | 19 | int test1; |
Rushu | 49:7eac11914980 | 20 | int test_jointround_flag=0; |
shaorui | 48:1b51771c3647 | 21 | int stop_sign=0; |
shaorui | 51:adc72e125b15 | 22 | int NCycle; |
benkatz | 0:4e1c4df6aabd | 23 | #include "mbed.h" |
benkatz | 0:4e1c4df6aabd | 24 | #include "PositionSensor.h" |
benkatz | 20:bf9ea5125d52 | 25 | #include "structs.h" |
benkatz | 20:bf9ea5125d52 | 26 | #include "foc.h" |
benkatz | 22:60276ba87ac6 | 27 | #include "calibration.h" |
benkatz | 20:bf9ea5125d52 | 28 | #include "hw_setup.h" |
benkatz | 23:2adf23ee0305 | 29 | #include "math_ops.h" |
benkatz | 20:bf9ea5125d52 | 30 | #include "current_controller_config.h" |
benkatz | 20:bf9ea5125d52 | 31 | #include "hw_config.h" |
benkatz | 20:bf9ea5125d52 | 32 | #include "motor_config.h" |
benkatz | 23:2adf23ee0305 | 33 | #include "stm32f4xx_flash.h" |
benkatz | 23:2adf23ee0305 | 34 | #include "FlashWriter.h" |
benkatz | 23:2adf23ee0305 | 35 | #include "user_config.h" |
benkatz | 23:2adf23ee0305 | 36 | #include "PreferenceWriter.h" |
benkatz | 42:738fa01b0346 | 37 | #include "CAN_com.h" |
shaorui | 48:1b51771c3647 | 38 | #include "math.h" |
shaorui | 48:1b51771c3647 | 39 | #include "MA700Sensor.h" |
shaorui | 48:1b51771c3647 | 40 | #include "joint_calibration.h" |
shaorui | 48:1b51771c3647 | 41 | PreferenceWriter prefs(6); |
shaorui | 48:1b51771c3647 | 42 | //PreferenceWriter prefs(7); |
benkatz | 37:c0f352d6e8e3 | 43 | |
benkatz | 9:d7eb815cb057 | 44 | |
benkatz | 20:bf9ea5125d52 | 45 | GPIOStruct gpio; |
benkatz | 20:bf9ea5125d52 | 46 | ControllerStruct controller; |
benkatz | 20:bf9ea5125d52 | 47 | COMStruct com; |
benkatz | 37:c0f352d6e8e3 | 48 | ObserverStruct observer; |
benkatz | 43:dfb72608639c | 49 | Serial pc(PA_2, PA_3); |
benkatz | 9:d7eb815cb057 | 50 | |
Rushu | 47:55bdc4d5096b | 51 | CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name, 1000kbps |
benkatz | 26:2b865c00d7e9 | 52 | CANMessage rxMsg; |
benkatz | 26:2b865c00d7e9 | 53 | CANMessage txMsg; |
benkatz | 23:2adf23ee0305 | 54 | |
shaorui | 48:1b51771c3647 | 55 | int i=1;//shaorui add |
shaorui | 48:1b51771c3647 | 56 | float wucha=0; |
shaorui | 48:1b51771c3647 | 57 | float wucha1=0; |
Rushu | 47:55bdc4d5096b | 58 | PositionSensorAM5147 spi(16384, 0.0, NPP); //14 bits encoder, 21 NPP |
shaorui | 48:1b51771c3647 | 59 | PositionSensorMA700 ma700(16384,0.0,NPP); //shaorui add(12/10) |
benkatz | 20:bf9ea5125d52 | 60 | |
benkatz | 23:2adf23ee0305 | 61 | volatile int count = 0; |
benkatz | 23:2adf23ee0305 | 62 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 63 | volatile int state_change; |
Rushu | 49:7eac11914980 | 64 | volatile float Joint_init =0; //Joint intial angle |
Rushu | 49:7eac11914980 | 65 | volatile int J_M_flag = 0; // Joint motor angle combine |
benkatz | 20:bf9ea5125d52 | 66 | |
benkatz | 26:2b865c00d7e9 | 67 | void onMsgReceived() { |
benkatz | 26:2b865c00d7e9 | 68 | //msgAvailable = true; |
benkatz | 45:aadebe074af6 | 69 | |
benkatz | 26:2b865c00d7e9 | 70 | can.read(rxMsg); |
benkatz | 28:8c7e29f719c5 | 71 | if((rxMsg.id == CAN_ID)){ |
benkatz | 28:8c7e29f719c5 | 72 | controller.timeout = 0; |
benkatz | 28:8c7e29f719c5 | 73 | if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){ |
benkatz | 28:8c7e29f719c5 | 74 | state = MOTOR_MODE; |
benkatz | 28:8c7e29f719c5 | 75 | state_change = 1; |
benkatz | 28:8c7e29f719c5 | 76 | } |
benkatz | 28:8c7e29f719c5 | 77 | else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){ |
benkatz | 28:8c7e29f719c5 | 78 | state = REST_MODE; |
benkatz | 28:8c7e29f719c5 | 79 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 80 | gpio.led->write(0);; |
benkatz | 28:8c7e29f719c5 | 81 | } |
benkatz | 28:8c7e29f719c5 | 82 | else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){ |
benkatz | 28:8c7e29f719c5 | 83 | spi.ZeroPosition(); |
benkatz | 28:8c7e29f719c5 | 84 | } |
benkatz | 28:8c7e29f719c5 | 85 | else if(state == MOTOR_MODE){ |
benkatz | 28:8c7e29f719c5 | 86 | unpack_cmd(rxMsg, &controller); |
shaorui | 48:1b51771c3647 | 87 | /* |
shaorui | 48:1b51771c3647 | 88 | if(controller.sidebct1!=controller.sidebct) |
shaorui | 48:1b51771c3647 | 89 | { |
shaorui | 48:1b51771c3647 | 90 | controller.sidebct1=controller.sidebct; |
shaorui | 48:1b51771c3647 | 91 | ma700.WriteRegister(&controller); |
shaorui | 48:1b51771c3647 | 92 | } |
shaorui | 48:1b51771c3647 | 93 | */ |
benkatz | 28:8c7e29f719c5 | 94 | } |
shaorui | 48:1b51771c3647 | 95 | |
benkatz | 37:c0f352d6e8e3 | 96 | pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); |
benkatz | 37:c0f352d6e8e3 | 97 | can.write(txMsg); |
benkatz | 28:8c7e29f719c5 | 98 | } |
benkatz | 26:2b865c00d7e9 | 99 | |
benkatz | 26:2b865c00d7e9 | 100 | } |
benkatz | 26:2b865c00d7e9 | 101 | |
benkatz | 23:2adf23ee0305 | 102 | void enter_menu_state(void){ |
benkatz | 23:2adf23ee0305 | 103 | printf("\n\r\n\r\n\r"); |
benkatz | 23:2adf23ee0305 | 104 | printf(" Commands:\n\r"); |
benkatz | 44:efcde0af8390 | 105 | wait_us(10); |
benkatz | 26:2b865c00d7e9 | 106 | printf(" m - Motor Mode\n\r"); |
benkatz | 44:efcde0af8390 | 107 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 108 | printf(" c - Calibrate Encoder\n\r"); |
benkatz | 44:efcde0af8390 | 109 | wait_us(10); |
shaorui | 48:1b51771c3647 | 110 | printf(" j - Joint Calibrate Encoder\n\r"); |
shaorui | 48:1b51771c3647 | 111 | wait_us(10); |
shaorui | 48:1b51771c3647 | 112 | printf(" t - Joint test Encoder\n\r"); |
shaorui | 48:1b51771c3647 | 113 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 114 | printf(" s - Setup\n\r"); |
benkatz | 44:efcde0af8390 | 115 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 116 | printf(" e - Display Encoder\n\r"); |
benkatz | 44:efcde0af8390 | 117 | wait_us(10); |
benkatz | 37:c0f352d6e8e3 | 118 | printf(" z - Set Zero Position\n\r"); |
benkatz | 44:efcde0af8390 | 119 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 120 | printf(" esc - Exit to Menu\n\r"); |
benkatz | 44:efcde0af8390 | 121 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 122 | state_change = 0; |
benkatz | 25:f5741040c4bb | 123 | gpio.enable->write(0); |
benkatz | 37:c0f352d6e8e3 | 124 | gpio.led->write(0); |
shaorui | 51:adc72e125b15 | 125 | test_jointround_flag=0; |
shaorui | 51:adc72e125b15 | 126 | |
benkatz | 23:2adf23ee0305 | 127 | } |
benkatz | 24:58c2d7571207 | 128 | |
benkatz | 24:58c2d7571207 | 129 | void enter_setup_state(void){ |
benkatz | 24:58c2d7571207 | 130 | printf("\n\r\n\r Configuration Options \n\r\n\n"); |
benkatz | 44:efcde0af8390 | 131 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 132 | printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 44:efcde0af8390 | 133 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 134 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
benkatz | 44:efcde0af8390 | 135 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 136 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 44:efcde0af8390 | 137 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 138 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); |
benkatz | 44:efcde0af8390 | 139 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 140 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); |
benkatz | 44:efcde0af8390 | 141 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 142 | printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); |
benkatz | 44:efcde0af8390 | 143 | wait_us(10); |
benkatz | 24:58c2d7571207 | 144 | printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 44:efcde0af8390 | 145 | wait_us(10); |
benkatz | 24:58c2d7571207 | 146 | state_change = 0; |
benkatz | 24:58c2d7571207 | 147 | } |
benkatz | 22:60276ba87ac6 | 148 | |
benkatz | 23:2adf23ee0305 | 149 | void enter_torque_mode(void){ |
benkatz | 37:c0f352d6e8e3 | 150 | controller.ovp_flag = 0; |
benkatz | 28:8c7e29f719c5 | 151 | gpio.enable->write(1); // Enable gate drive |
benkatz | 28:8c7e29f719c5 | 152 | reset_foc(&controller); // Tesets integrators, and other control loop parameters |
benkatz | 28:8c7e29f719c5 | 153 | wait(.001); |
benkatz | 23:2adf23ee0305 | 154 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 155 | controller.i_q_ref = 0; // Current Setpoints |
benkatz | 37:c0f352d6e8e3 | 156 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 157 | state_change = 0; |
benkatz | 28:8c7e29f719c5 | 158 | printf("\n\r Entering Motor Mode \n\r"); |
benkatz | 23:2adf23ee0305 | 159 | } |
benkatz | 22:60276ba87ac6 | 160 | |
benkatz | 23:2adf23ee0305 | 161 | void calibrate(void){ |
benkatz | 25:f5741040c4bb | 162 | gpio.enable->write(1); // Enable gate drive |
benkatz | 37:c0f352d6e8e3 | 163 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 164 | order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
benkatz | 25:f5741040c4bb | 165 | calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
shaorui | 48:1b51771c3647 | 166 | //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs); |
shaorui | 48:1b51771c3647 | 167 | //j_calibrate(&ma700,&gpio, &controller, &prefs); |
benkatz | 37:c0f352d6e8e3 | 168 | gpio.led->write(0);; // Turn off status LED |
benkatz | 23:2adf23ee0305 | 169 | wait(.2); |
benkatz | 25:f5741040c4bb | 170 | gpio.enable->write(0); // Turn off gate drive |
benkatz | 23:2adf23ee0305 | 171 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 23:2adf23ee0305 | 172 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 173 | } |
benkatz | 23:2adf23ee0305 | 174 | |
shaorui | 48:1b51771c3647 | 175 | void jocalibrate(void){ |
shaorui | 48:1b51771c3647 | 176 | gpio.enable->write(1); // Enable gate drive |
shaorui | 48:1b51771c3647 | 177 | gpio.led->write(1); // Turn on status LED |
Rushu | 49:7eac11914980 | 178 | |
Rushu | 49:7eac11914980 | 179 | //order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
shaorui | 48:1b51771c3647 | 180 | //calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
shaorui | 48:1b51771c3647 | 181 | //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs); |
shaorui | 48:1b51771c3647 | 182 | j_calibrate(&ma700,&gpio, &controller, &prefs); |
Rushu | 49:7eac11914980 | 183 | |
Rushu | 49:7eac11914980 | 184 | /*************同时设置转子和关节零位置同步****************///暂时不需要,标定结束,按z归零 |
Rushu | 49:7eac11914980 | 185 | /* |
Rushu | 49:7eac11914980 | 186 | spi.SetMechOffset(0); |
Rushu | 49:7eac11914980 | 187 | ma700.SetMechOffset(0); |
Rushu | 49:7eac11914980 | 188 | spi.Sample(DT); |
Rushu | 49:7eac11914980 | 189 | ma700.Sample(DT); |
Rushu | 49:7eac11914980 | 190 | wait_us(20); |
Rushu | 49:7eac11914980 | 191 | M_OFFSET = spi.GetMechPosition(); |
Rushu | 49:7eac11914980 | 192 | JOINT_M_OFFSET =ma700.GetMechPosition(); |
Rushu | 49:7eac11914980 | 193 | if (!prefs.ready()) prefs.open(); |
Rushu | 49:7eac11914980 | 194 | prefs.flush(); // Write new prefs to flash |
Rushu | 49:7eac11914980 | 195 | prefs.close(); |
Rushu | 49:7eac11914980 | 196 | prefs.load(); |
Rushu | 49:7eac11914980 | 197 | spi.SetMechOffset(M_OFFSET); |
Rushu | 49:7eac11914980 | 198 | ma700.SetMechOffset(JOINT_M_OFFSET); |
Rushu | 49:7eac11914980 | 199 | printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
Rushu | 49:7eac11914980 | 200 | printf("\n\r Saved new zero position1: %.4f\n\r\n\r", JOINT_M_OFFSET); |
Rushu | 49:7eac11914980 | 201 | */ |
Rushu | 49:7eac11914980 | 202 | /*************同时设置转子和关节零位置同步****************/ |
Rushu | 49:7eac11914980 | 203 | gpio.led->write(0); // Turn off status LED |
shaorui | 48:1b51771c3647 | 204 | wait(.2); |
shaorui | 48:1b51771c3647 | 205 | gpio.enable->write(0); // Turn off gate drive |
shaorui | 48:1b51771c3647 | 206 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
shaorui | 48:1b51771c3647 | 207 | state_change = 0; |
shaorui | 48:1b51771c3647 | 208 | } |
shaorui | 48:1b51771c3647 | 209 | |
shaorui | 48:1b51771c3647 | 210 | void jointcalibrate(void){ |
shaorui | 48:1b51771c3647 | 211 | gpio.enable->write(1); // Enable gate drive |
shaorui | 48:1b51771c3647 | 212 | gpio.led->write(1); // Turn on status LED |
shaorui | 48:1b51771c3647 | 213 | //joint_calibrate (&ma700,&spi,&gpio,&controller,&prefs); // Perform calibration procedure |
shaorui | 48:1b51771c3647 | 214 | gpio.led->write(0); // Turn off status LED |
shaorui | 48:1b51771c3647 | 215 | wait(.2); |
shaorui | 48:1b51771c3647 | 216 | gpio.enable->write(0); |
shaorui | 48:1b51771c3647 | 217 | |
shaorui | 48:1b51771c3647 | 218 | |
shaorui | 48:1b51771c3647 | 219 | |
shaorui | 48:1b51771c3647 | 220 | /************Trajectory Planning******************************/ |
shaorui | 48:1b51771c3647 | 221 | |
shaorui | 48:1b51771c3647 | 222 | |
shaorui | 48:1b51771c3647 | 223 | // enter_torque_mode(); |
shaorui | 48:1b51771c3647 | 224 | state=MOTOR_MODE; |
shaorui | 48:1b51771c3647 | 225 | state_change=1; |
shaorui | 48:1b51771c3647 | 226 | //enter_torque_mode(); |
shaorui | 48:1b51771c3647 | 227 | count = 0; |
shaorui | 48:1b51771c3647 | 228 | printf("test\n\r"); |
Rushu | 49:7eac11914980 | 229 | controller.p_des=0; |
Rushu | 49:7eac11914980 | 230 | controller.v_des = 1.5f; |
Rushu | 49:7eac11914980 | 231 | controller.kp = 0; |
Rushu | 49:7eac11914980 | 232 | controller.kd = 5.0f; |
Rushu | 49:7eac11914980 | 233 | controller.t_ff = 0; |
shaorui | 48:1b51771c3647 | 234 | /* |
shaorui | 48:1b51771c3647 | 235 | if((1.0f/GR)* spi.GetMechPosition()<=(2*PI)) |
shaorui | 48:1b51771c3647 | 236 | { |
shaorui | 48:1b51771c3647 | 237 | controller.p_des=0; |
shaorui | 48:1b51771c3647 | 238 | controller.v_des = 2.0f; |
shaorui | 48:1b51771c3647 | 239 | controller.kp = 0; |
shaorui | 48:1b51771c3647 | 240 | controller.kd = 5.0f; |
shaorui | 48:1b51771c3647 | 241 | controller.t_ff = 0; |
shaorui | 48:1b51771c3647 | 242 | wait(.5); |
shaorui | 48:1b51771c3647 | 243 | * |
shaorui | 48:1b51771c3647 | 244 | } |
shaorui | 48:1b51771c3647 | 245 | |
shaorui | 48:1b51771c3647 | 246 | |
shaorui | 48:1b51771c3647 | 247 | ************Trajectory Planning*****************************/ |
shaorui | 48:1b51771c3647 | 248 | |
shaorui | 48:1b51771c3647 | 249 | // Turn off gate drive |
shaorui | 48:1b51771c3647 | 250 | printf("\n\r Joint_Calibration complete. Press 'esc' to return to menu\n\r"); |
shaorui | 48:1b51771c3647 | 251 | //state_change = 0; |
shaorui | 48:1b51771c3647 | 252 | } |
shaorui | 48:1b51771c3647 | 253 | |
shaorui | 48:1b51771c3647 | 254 | |
shaorui | 48:1b51771c3647 | 255 | |
benkatz | 23:2adf23ee0305 | 256 | void print_encoder(void){ |
Rushu | 49:7eac11914980 | 257 | printf(" AS5147Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); |
shaorui | 51:adc72e125b15 | 258 | // printf(" MA700:Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", ma700.GetMechPosition(), ma700.GetElecPosition(), ma700.GetRawPosition()); |
shaorui | 51:adc72e125b15 | 259 | // printf(" Joint Angle: %f Mech1: %f Mech: %f\n\r", controller.theta_joint_raw*57.2957795, -controller.theta_mech1*57.2957795*GR, controller.theta_mech*57.2957795*GR); |
Rushu | 49:7eac11914980 | 260 | |
Rushu | 49:7eac11914980 | 261 | // float m_position=controller.theta_mech*57.2957795; |
Rushu | 49:7eac11914980 | 262 | // float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR; |
Rushu | 49:7eac11914980 | 263 | // float j_position=-(controller.theta_mech1+controller.theta_mech)*2807.49319614; |
Rushu | 49:7eac11914980 | 264 | wait(.5); |
benkatz | 22:60276ba87ac6 | 265 | } |
benkatz | 20:bf9ea5125d52 | 266 | |
benkatz | 23:2adf23ee0305 | 267 | /// Current Sampling Interrupt /// |
benkatz | 23:2adf23ee0305 | 268 | /// This runs at 40 kHz, regardless of of the mode the controller is in /// |
benkatz | 2:8724412ad628 | 269 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 270 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 23:2adf23ee0305 | 271 | |
benkatz | 23:2adf23ee0305 | 272 | ///Sample current always /// |
benkatz | 25:f5741040c4bb | 273 | ADC1->CR2 |= 0x40000000; // Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 274 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 275 | //for (delay = 0; delay < 55; delay++); |
benkatz | 37:c0f352d6e8e3 | 276 | controller.adc2_raw = ADC2->DR; // Read ADC Data Registers |
benkatz | 23:2adf23ee0305 | 277 | controller.adc1_raw = ADC1->DR; |
benkatz | 37:c0f352d6e8e3 | 278 | controller.adc3_raw = ADC3->DR; |
shaorui | 48:1b51771c3647 | 279 | spi.Sample(DT); |
shaorui | 48:1b51771c3647 | 280 | ma700.Sample(DT); // sample position sensor |
benkatz | 37:c0f352d6e8e3 | 281 | controller.theta_elec = spi.GetElecPosition(); |
Rushu | 49:7eac11914980 | 282 | // controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 283 | controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); |
benkatz | 37:c0f352d6e8e3 | 284 | controller.dtheta_elec = spi.GetElecVelocity(); |
benkatz | 37:c0f352d6e8e3 | 285 | controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; |
shaorui | 48:1b51771c3647 | 286 | |
benkatz | 20:bf9ea5125d52 | 287 | |
shaorui | 48:1b51771c3647 | 288 | //////shaorui add for obtaining joint real position |
shaorui | 48:1b51771c3647 | 289 | controller.theta_elec1 = ma700.GetElecPosition(); |
Rushu | 49:7eac11914980 | 290 | controller.theta_mech1 = (1.0f/GR)*ma700.GetMechPosition(); |
Rushu | 49:7eac11914980 | 291 | controller.dtheta_mech1 =(1.0f/GR)*ma700.GetMechVelocity(); |
shaorui | 48:1b51771c3647 | 292 | controller.dtheta_elec1 = ma700.GetElecVelocity(); |
Rushu | 49:7eac11914980 | 293 | |
Rushu | 49:7eac11914980 | 294 | ///hjb add joint angle |
shaorui | 51:adc72e125b15 | 295 | controller.theta_joint_raw= -(ma700.GetMechPosition()+spi.GetMechPosition())+0.0134; |
shaorui | 51:adc72e125b15 | 296 | /* |
Rushu | 49:7eac11914980 | 297 | if(controller.dtheta_mech>0) {controller.theta_joint_raw += PI/180.0f;} //compensate |
Rushu | 49:7eac11914980 | 298 | else if(controller.dtheta_mech<0) {controller.theta_joint_raw -= PI/180.0f;} |
shaorui | 51:adc72e125b15 | 299 | */ |
Rushu | 49:7eac11914980 | 300 | if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;} |
Rushu | 49:7eac11914980 | 301 | if(controller.theta_joint_raw>=360){controller.theta_joint_raw -= 2.0f*PI;} |
Rushu | 49:7eac11914980 | 302 | |
Rushu | 49:7eac11914980 | 303 | controller.theta_joint_raw_fil =0.99f*controller.theta_joint_raw_pre + 0.01f*controller.theta_joint_raw ;//filter |
Rushu | 49:7eac11914980 | 304 | controller.theta_joint_raw_pre =controller.theta_joint_raw_fil; |
shaorui | 51:adc72e125b15 | 305 | |
Rushu | 49:7eac11914980 | 306 | //"rad 0~2pi" ("deg" =-(controller.theta_mech1+controller.theta_mech)*2807.49319614; |
Rushu | 49:7eac11914980 | 307 | //int Ncycle=0; |
Rushu | 49:7eac11914980 | 308 | //float Ncycle_mod = 0.0f; |
Rushu | 49:7eac11914980 | 309 | //float Mech_mod = 0.0f; |
shaorui | 51:adc72e125b15 | 310 | NCycle=controller.Ncycle = controller.theta_joint_raw/(2.0f*PI/GR); |
Rushu | 49:7eac11914980 | 311 | controller.Ncycle_mod = fmod(controller.theta_joint_raw,2.0f*PI/GR); |
Rushu | 49:7eac11914980 | 312 | controller.Mech_mod = fmod(spi.GetMechPosition(),2.0f*PI); |
shaorui | 51:adc72e125b15 | 313 | /* |
shaorui | 51:adc72e125b15 | 314 | if(abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)>2.5) //not in the error range |
Rushu | 49:7eac11914980 | 315 | { |
shaorui | 51:adc72e125b15 | 316 | if ((2*PI-0.1)>controller.Mech_mod>PI) // |
Rushu | 49:7eac11914980 | 317 | { |
Rushu | 49:7eac11914980 | 318 | controller.Ncycle -= 1; |
Rushu | 49:7eac11914980 | 319 | } |
shaorui | 51:adc72e125b15 | 320 | else if(0.1<controller.Mech_mod<PI) // |
Rushu | 49:7eac11914980 | 321 | { |
Rushu | 49:7eac11914980 | 322 | controller.Ncycle += 1; |
Rushu | 49:7eac11914980 | 323 | } |
Rushu | 49:7eac11914980 | 324 | |
Rushu | 49:7eac11914980 | 325 | } |
shaorui | 51:adc72e125b15 | 326 | */ |
Rushu | 49:7eac11914980 | 327 | controller.theta_joint= controller.Ncycle*(2.0f*PI/GR)+controller.Mech_mod/GR; //In the real use, should turn to the theta_mech |
Rushu | 49:7eac11914980 | 328 | if(J_M_flag == 0) |
Rushu | 49:7eac11914980 | 329 | { |
shaorui | 51:adc72e125b15 | 330 | if(abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)<2) |
shaorui | 51:adc72e125b15 | 331 | { |
shaorui | 51:adc72e125b15 | 332 | if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<2) |
shaorui | 51:adc72e125b15 | 333 | { |
shaorui | 51:adc72e125b15 | 334 | Joint_init = controller.theta_joint- (1.0f/GR)*spi.GetMechPosition(); |
shaorui | 51:adc72e125b15 | 335 | controller.theta_mech = controller.theta_joint;////controller.theta_joint; // easy way to use, whether available in the shock? |
shaorui | 51:adc72e125b15 | 336 | J_M_flag = 1; |
shaorui | 51:adc72e125b15 | 337 | } |
Rushu | 49:7eac11914980 | 338 | } |
Rushu | 49:7eac11914980 | 339 | controller.theta_mech = controller.theta_joint_raw; |
Rushu | 49:7eac11914980 | 340 | } |
Rushu | 49:7eac11914980 | 341 | else if(J_M_flag == 1) |
Rushu | 49:7eac11914980 | 342 | { |
Rushu | 49:7eac11914980 | 343 | controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition(); |
Rushu | 49:7eac11914980 | 344 | } |
Rushu | 49:7eac11914980 | 345 | |
Rushu | 49:7eac11914980 | 346 | |
shaorui | 48:1b51771c3647 | 347 | /////shaorui end////////////////// |
shaorui | 48:1b51771c3647 | 348 | /* |
shaorui | 48:1b51771c3647 | 349 | controller.c++; |
shaorui | 48:1b51771c3647 | 350 | if(controller.c>=20000) |
shaorui | 48:1b51771c3647 | 351 | { |
shaorui | 48:1b51771c3647 | 352 | controller.cha=controller.init2-controller.init1; |
shaorui | 48:1b51771c3647 | 353 | controller.init1=controller.init2; |
shaorui | 48:1b51771c3647 | 354 | controller.c=0; |
shaorui | 48:1b51771c3647 | 355 | printf("position: %.3f \n\r", controller.cha*360/(2.0f*PI)); |
shaorui | 48:1b51771c3647 | 356 | } |
shaorui | 48:1b51771c3647 | 357 | */ |
benkatz | 23:2adf23ee0305 | 358 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 359 | switch(state){ |
benkatz | 37:c0f352d6e8e3 | 360 | case REST_MODE: // Do nothing |
benkatz | 23:2adf23ee0305 | 361 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 362 | enter_menu_state(); |
shaorui | 48:1b51771c3647 | 363 | wucha=0 ; //shaorui add |
benkatz | 23:2adf23ee0305 | 364 | } |
benkatz | 23:2adf23ee0305 | 365 | break; |
benkatz | 22:60276ba87ac6 | 366 | |
benkatz | 23:2adf23ee0305 | 367 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 368 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 369 | calibrate(); |
benkatz | 23:2adf23ee0305 | 370 | } |
benkatz | 23:2adf23ee0305 | 371 | break; |
benkatz | 23:2adf23ee0305 | 372 | |
shaorui | 48:1b51771c3647 | 373 | case J_CALIBRATION_MODE: // Run encoder calibration procedure |
shaorui | 48:1b51771c3647 | 374 | if(state_change){ |
shaorui | 48:1b51771c3647 | 375 | jocalibrate(); |
shaorui | 48:1b51771c3647 | 376 | |
shaorui | 48:1b51771c3647 | 377 | } |
shaorui | 48:1b51771c3647 | 378 | break; |
shaorui | 48:1b51771c3647 | 379 | |
Rushu | 49:7eac11914980 | 380 | case TEST_TRAJECTORY_MODE: // Run encoder calibration procedure |
shaorui | 48:1b51771c3647 | 381 | if(state_change){ |
Rushu | 49:7eac11914980 | 382 | test_jointround_flag=1; |
Rushu | 49:7eac11914980 | 383 | J_M_flag = 0; |
shaorui | 48:1b51771c3647 | 384 | stop_sign=0; |
shaorui | 48:1b51771c3647 | 385 | jointcalibrate(); |
shaorui | 48:1b51771c3647 | 386 | |
shaorui | 48:1b51771c3647 | 387 | } |
shaorui | 48:1b51771c3647 | 388 | break; |
shaorui | 48:1b51771c3647 | 389 | |
shaorui | 48:1b51771c3647 | 390 | |
benkatz | 26:2b865c00d7e9 | 391 | case MOTOR_MODE: // Run torque control |
benkatz | 25:f5741040c4bb | 392 | if(state_change){ |
benkatz | 25:f5741040c4bb | 393 | enter_torque_mode(); |
benkatz | 28:8c7e29f719c5 | 394 | count = 0; |
benkatz | 25:f5741040c4bb | 395 | } |
benkatz | 28:8c7e29f719c5 | 396 | else{ |
benkatz | 37:c0f352d6e8e3 | 397 | /* |
benkatz | 37:c0f352d6e8e3 | 398 | if(controller.v_bus>28.0f){ //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen |
benkatz | 37:c0f352d6e8e3 | 399 | gpio.enable->write(0); |
benkatz | 37:c0f352d6e8e3 | 400 | controller.ovp_flag = 1; |
benkatz | 37:c0f352d6e8e3 | 401 | state = REST_MODE; |
benkatz | 37:c0f352d6e8e3 | 402 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 403 | printf("OVP Triggered!\n\r"); |
benkatz | 37:c0f352d6e8e3 | 404 | } |
benkatz | 37:c0f352d6e8e3 | 405 | */ |
benkatz | 37:c0f352d6e8e3 | 406 | |
shaorui | 48:1b51771c3647 | 407 | torque_control(&controller); |
shaorui | 51:adc72e125b15 | 408 | /* |
shaorui | 51:adc72e125b15 | 409 | if(test_jointround_flag!=1) |
shaorui | 51:adc72e125b15 | 410 | { |
benkatz | 28:8c7e29f719c5 | 411 | if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ |
benkatz | 28:8c7e29f719c5 | 412 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 413 | controller.i_q_ref = 0; |
benkatz | 37:c0f352d6e8e3 | 414 | controller.kp = 0; |
benkatz | 37:c0f352d6e8e3 | 415 | controller.kd = 0; |
benkatz | 37:c0f352d6e8e3 | 416 | controller.t_ff = 0; |
benkatz | 28:8c7e29f719c5 | 417 | } |
shaorui | 51:adc72e125b15 | 418 | } |
shaorui | 51:adc72e125b15 | 419 | */ |
benkatz | 37:c0f352d6e8e3 | 420 | commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop |
benkatz | 28:8c7e29f719c5 | 421 | controller.timeout += 1; |
benkatz | 38:67e4e1453a4b | 422 | |
Rushu | 49:7eac11914980 | 423 | |
Rushu | 49:7eac11914980 | 424 | count++; |
benkatz | 39:3580a907ef93 | 425 | /* |
benkatz | 40:cd7e837b2b93 | 426 | if(count == 4000){ |
benkatz | 40:cd7e837b2b93 | 427 | printf("%.4f\n\r", controller.dtheta_mech); |
benkatz | 32:ccac5da77844 | 428 | count = 0; |
benkatz | 23:2adf23ee0305 | 429 | } |
benkatz | 39:3580a907ef93 | 430 | */ |
benkatz | 38:67e4e1453a4b | 431 | |
benkatz | 37:c0f352d6e8e3 | 432 | |
benkatz | 37:c0f352d6e8e3 | 433 | } |
benkatz | 23:2adf23ee0305 | 434 | break; |
benkatz | 23:2adf23ee0305 | 435 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 436 | if(state_change){ |
benkatz | 24:58c2d7571207 | 437 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 438 | } |
benkatz | 23:2adf23ee0305 | 439 | break; |
benkatz | 23:2adf23ee0305 | 440 | case ENCODER_MODE: |
shaorui | 51:adc72e125b15 | 441 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 442 | break; |
benkatz | 37:c0f352d6e8e3 | 443 | } |
benkatz | 2:8724412ad628 | 444 | } |
benkatz | 23:2adf23ee0305 | 445 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 446 | } |
benkatz | 0:4e1c4df6aabd | 447 | |
benkatz | 25:f5741040c4bb | 448 | |
benkatz | 24:58c2d7571207 | 449 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 450 | char cmd_id = 0; |
benkatz | 25:f5741040c4bb | 451 | char char_count = 0; |
benkatz | 24:58c2d7571207 | 452 | |
benkatz | 25:f5741040c4bb | 453 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 25:f5741040c4bb | 454 | /// Called when data received over serial /// |
benkatz | 23:2adf23ee0305 | 455 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 456 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 457 | char c = pc.getc(); |
benkatz | 25:f5741040c4bb | 458 | if(c == 27){ |
benkatz | 25:f5741040c4bb | 459 | state = REST_MODE; |
benkatz | 25:f5741040c4bb | 460 | state_change = 1; |
benkatz | 25:f5741040c4bb | 461 | char_count = 0; |
benkatz | 25:f5741040c4bb | 462 | cmd_id = 0; |
shaorui | 51:adc72e125b15 | 463 | gpio.led->write(0); |
benkatz | 25:f5741040c4bb | 464 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 25:f5741040c4bb | 465 | } |
benkatz | 24:58c2d7571207 | 466 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 467 | switch (c){ |
benkatz | 23:2adf23ee0305 | 468 | case 'c': |
benkatz | 23:2adf23ee0305 | 469 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 470 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 471 | break; |
shaorui | 48:1b51771c3647 | 472 | |
shaorui | 48:1b51771c3647 | 473 | case 't': |
Rushu | 49:7eac11914980 | 474 | state = TEST_TRAJECTORY_MODE; |
shaorui | 48:1b51771c3647 | 475 | state_change = 1; |
shaorui | 48:1b51771c3647 | 476 | break; |
shaorui | 48:1b51771c3647 | 477 | case 'j': |
shaorui | 48:1b51771c3647 | 478 | state = J_CALIBRATION_MODE; |
shaorui | 48:1b51771c3647 | 479 | state_change = 1; |
shaorui | 48:1b51771c3647 | 480 | break; |
shaorui | 48:1b51771c3647 | 481 | |
benkatz | 26:2b865c00d7e9 | 482 | case 'm': |
benkatz | 26:2b865c00d7e9 | 483 | state = MOTOR_MODE; |
benkatz | 23:2adf23ee0305 | 484 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 485 | break; |
benkatz | 23:2adf23ee0305 | 486 | case 'e': |
benkatz | 23:2adf23ee0305 | 487 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 488 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 489 | break; |
benkatz | 23:2adf23ee0305 | 490 | case 's': |
benkatz | 23:2adf23ee0305 | 491 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 492 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 493 | break; |
shaorui | 48:1b51771c3647 | 494 | |
benkatz | 37:c0f352d6e8e3 | 495 | case 'z': |
benkatz | 37:c0f352d6e8e3 | 496 | spi.SetMechOffset(0); |
shaorui | 48:1b51771c3647 | 497 | ma700.SetMechOffset(0); |
benkatz | 45:aadebe074af6 | 498 | spi.Sample(DT); |
shaorui | 48:1b51771c3647 | 499 | ma700.Sample(DT); |
benkatz | 37:c0f352d6e8e3 | 500 | wait_us(20); |
benkatz | 37:c0f352d6e8e3 | 501 | M_OFFSET = spi.GetMechPosition(); |
shaorui | 48:1b51771c3647 | 502 | JOINT_M_OFFSET = ma700.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 503 | if (!prefs.ready()) prefs.open(); |
benkatz | 37:c0f352d6e8e3 | 504 | prefs.flush(); // Write new prefs to flash |
benkatz | 37:c0f352d6e8e3 | 505 | prefs.close(); |
benkatz | 37:c0f352d6e8e3 | 506 | prefs.load(); |
benkatz | 37:c0f352d6e8e3 | 507 | spi.SetMechOffset(M_OFFSET); |
shaorui | 48:1b51771c3647 | 508 | ma700.SetMechOffset(JOINT_M_OFFSET); |
Rushu | 49:7eac11914980 | 509 | printf("\n\r Saved new M-J J-J zero position: %.4f %.4f\n\r\n\r", M_OFFSET, JOINT_M_OFFSET); |
shaorui | 51:adc72e125b15 | 510 | |
shaorui | 48:1b51771c3647 | 511 | for(int i=0;i<300;i++) |
shaorui | 48:1b51771c3647 | 512 | { |
shaorui | 48:1b51771c3647 | 513 | printf("%.3d %.3d\n\r",i,__int_reg[i] ); |
shaorui | 48:1b51771c3647 | 514 | } |
shaorui | 48:1b51771c3647 | 515 | for(int j=0;j<64;j++) |
shaorui | 48:1b51771c3647 | 516 | { |
shaorui | 48:1b51771c3647 | 517 | printf("%.3d %.3f\n\r",j,__float_reg[j] ); |
shaorui | 48:1b51771c3647 | 518 | } |
shaorui | 51:adc72e125b15 | 519 | |
benkatz | 37:c0f352d6e8e3 | 520 | break; |
benkatz | 37:c0f352d6e8e3 | 521 | } |
benkatz | 37:c0f352d6e8e3 | 522 | |
benkatz | 24:58c2d7571207 | 523 | } |
benkatz | 24:58c2d7571207 | 524 | else if(state == SETUP_MODE){ |
benkatz | 25:f5741040c4bb | 525 | if(c == 13){ |
benkatz | 24:58c2d7571207 | 526 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 527 | case 'b': |
benkatz | 24:58c2d7571207 | 528 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 529 | break; |
benkatz | 24:58c2d7571207 | 530 | case 'i': |
benkatz | 24:58c2d7571207 | 531 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 532 | break; |
benkatz | 26:2b865c00d7e9 | 533 | case 'm': |
benkatz | 26:2b865c00d7e9 | 534 | CAN_MASTER = atoi(cmd_val); |
benkatz | 26:2b865c00d7e9 | 535 | break; |
benkatz | 24:58c2d7571207 | 536 | case 'l': |
benkatz | 24:58c2d7571207 | 537 | TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 538 | break; |
benkatz | 28:8c7e29f719c5 | 539 | case 't': |
benkatz | 28:8c7e29f719c5 | 540 | CAN_TIMEOUT = atoi(cmd_val); |
benkatz | 28:8c7e29f719c5 | 541 | break; |
benkatz | 24:58c2d7571207 | 542 | default: |
benkatz | 24:58c2d7571207 | 543 | printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 544 | break; |
benkatz | 24:58c2d7571207 | 545 | } |
benkatz | 24:58c2d7571207 | 546 | |
benkatz | 24:58c2d7571207 | 547 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 548 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 549 | prefs.close(); |
benkatz | 24:58c2d7571207 | 550 | prefs.load(); |
benkatz | 24:58c2d7571207 | 551 | state_change = 1; |
benkatz | 24:58c2d7571207 | 552 | char_count = 0; |
benkatz | 24:58c2d7571207 | 553 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 554 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 555 | } |
benkatz | 24:58c2d7571207 | 556 | else{ |
benkatz | 24:58c2d7571207 | 557 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 558 | else{ |
benkatz | 24:58c2d7571207 | 559 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 560 | |
benkatz | 24:58c2d7571207 | 561 | } |
benkatz | 24:58c2d7571207 | 562 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 563 | char_count++; |
benkatz | 23:2adf23ee0305 | 564 | } |
benkatz | 23:2adf23ee0305 | 565 | } |
benkatz | 24:58c2d7571207 | 566 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 567 | switch (c){ |
benkatz | 24:58c2d7571207 | 568 | case 27: |
benkatz | 24:58c2d7571207 | 569 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 570 | state_change = 1; |
benkatz | 24:58c2d7571207 | 571 | break; |
benkatz | 24:58c2d7571207 | 572 | } |
benkatz | 24:58c2d7571207 | 573 | } |
benkatz | 24:58c2d7571207 | 574 | |
benkatz | 24:58c2d7571207 | 575 | } |
benkatz | 22:60276ba87ac6 | 576 | } |
benkatz | 0:4e1c4df6aabd | 577 | |
benkatz | 0:4e1c4df6aabd | 578 | int main() { |
benkatz | 45:aadebe074af6 | 579 | |
benkatz | 20:bf9ea5125d52 | 580 | controller.v_bus = V_BUS; |
benkatz | 22:60276ba87ac6 | 581 | controller.mode = 0; |
shaorui | 48:1b51771c3647 | 582 | controller.sidebct1=0; |
benkatz | 23:2adf23ee0305 | 583 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 20:bf9ea5125d52 | 584 | |
benkatz | 9:d7eb815cb057 | 585 | wait(.1); |
benkatz | 26:2b865c00d7e9 | 586 | gpio.enable->write(1); |
benkatz | 45:aadebe074af6 | 587 | TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles |
benkatz | 45:aadebe074af6 | 588 | TIM1->CCR2 = PWM_ARR*(1.0f); |
benkatz | 45:aadebe074af6 | 589 | TIM1->CCR1 = PWM_ARR*(1.0f); |
benkatz | 23:2adf23ee0305 | 590 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
benkatz | 26:2b865c00d7e9 | 591 | gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 592 | reset_foc(&controller); // Reset current controller |
benkatz | 26:2b865c00d7e9 | 593 | TIM1->CR1 ^= TIM_CR1_UDIS; |
benkatz | 26:2b865c00d7e9 | 594 | //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 595 | |
benkatz | 20:bf9ea5125d52 | 596 | wait(.1); |
shaorui | 48:1b51771c3647 | 597 | NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0); // commutation > communication |
benkatz | 43:dfb72608639c | 598 | |
benkatz | 37:c0f352d6e8e3 | 599 | NVIC_SetPriority(CAN1_RX0_IRQn, 3); |
benkatz | 26:2b865c00d7e9 | 600 | can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
benkatz | 43:dfb72608639c | 601 | |
benkatz | 28:8c7e29f719c5 | 602 | txMsg.id = CAN_MASTER; |
benkatz | 28:8c7e29f719c5 | 603 | txMsg.len = 6; |
benkatz | 26:2b865c00d7e9 | 604 | rxMsg.len = 8; |
benkatz | 43:dfb72608639c | 605 | can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler |
shaorui | 51:adc72e125b15 | 606 | prefs.load(); |
shaorui | 51:adc72e125b15 | 607 | /* // Read flash |
benkatz | 37:c0f352d6e8e3 | 608 | if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} |
benkatz | 37:c0f352d6e8e3 | 609 | if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} |
benkatz | 25:f5741040c4bb | 610 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
benkatz | 37:c0f352d6e8e3 | 611 | spi.SetMechOffset(M_OFFSET); |
Rushu | 49:7eac11914980 | 612 | if(isnan(JOINT_E_OFFSET)){JOINT_E_OFFSET = 0.0f;} |
Rushu | 49:7eac11914980 | 613 | if(isnan(JOINT_M_OFFSET)){JOINT_M_OFFSET = 0.0f;} |
Rushu | 49:7eac11914980 | 614 | ma700.SetElecOffset(JOINT_E_OFFSET); // Set position sensor offset |
Rushu | 49:7eac11914980 | 615 | ma700.SetMechOffset(JOINT_M_OFFSET); |
shaorui | 51:adc72e125b15 | 616 | */ |
benkatz | 23:2adf23ee0305 | 617 | int lut[128] = {0}; |
Rushu | 49:7eac11914980 | 618 | int joint[128]={0}; //shaorui |
benkatz | 23:2adf23ee0305 | 619 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
shaorui | 48:1b51771c3647 | 620 | spi.WriteLUT(lut); |
shaorui | 48:1b51771c3647 | 621 | memcpy(&joint, &ENCODER_JOINT , sizeof(joint)); |
shaorui | 48:1b51771c3647 | 622 | spi.WriteLUT(joint); |
shaorui | 51:adc72e125b15 | 623 | pc.baud(115200);// |
shaorui | 51:adc72e125b15 | 624 | //pc.baud(921600); // set serial baud rate |
benkatz | 20:bf9ea5125d52 | 625 | wait(.01); |
Rushu | 49:7eac11914980 | 626 | pc.printf("\n\r\n\r QXSLAB Joint\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 627 | wait(.01); |
benkatz | 23:2adf23ee0305 | 628 | printf("\n\r Debug Info:\n\r"); |
benkatz | 32:ccac5da77844 | 629 | printf(" Firmware Version: %s\n\r", VERSION_NUM); |
benkatz | 23:2adf23ee0305 | 630 | printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
benkatz | 23:2adf23ee0305 | 631 | printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 632 | printf(" Output Zero Position: %.4f\n\r", M_OFFSET); |
shaorui | 51:adc72e125b15 | 633 | printf(" JointOutput Zero Position: %.4f\n\r", JOINT_M_OFFSET); |
benkatz | 24:58c2d7571207 | 634 | printf(" CAN ID: %d\n\r", CAN_ID); |
Rushu | 49:7eac11914980 | 635 | |
Rushu | 49:7eac11914980 | 636 | controller.theta_joint_raw_pre=-(ma700.GetMechPosition()+spi.GetMechPosition()); //hjb added, for joint encoder filter |
benkatz | 23:2adf23ee0305 | 637 | pc.attach(&serial_interrupt); // attach serial interrupt |
Rushu | 49:7eac11914980 | 638 | J_M_flag = 0; |
shaorui | 48:1b51771c3647 | 639 | //state_change = 1; |
benkatz | 20:bf9ea5125d52 | 640 | |
benkatz | 22:60276ba87ac6 | 641 | |
shaorui | 48:1b51771c3647 | 642 | while(1) { |
Rushu | 49:7eac11914980 | 643 | // wait(.1); |
shaorui | 48:1b51771c3647 | 644 | if(state == MOTOR_MODE) |
shaorui | 48:1b51771c3647 | 645 | { |
Rushu | 49:7eac11914980 | 646 | if(test_jointround_flag==1) |
shaorui | 48:1b51771c3647 | 647 | { |
Rushu | 49:7eac11914980 | 648 | if(controller.theta_joint_raw*57.2957795<=1) |
shaorui | 48:1b51771c3647 | 649 | { |
shaorui | 48:1b51771c3647 | 650 | //if(stop_sign==0) |
shaorui | 48:1b51771c3647 | 651 | //{ |
shaorui | 48:1b51771c3647 | 652 | controller.v_des = 0; |
shaorui | 48:1b51771c3647 | 653 | wait(1); |
shaorui | 48:1b51771c3647 | 654 | controller.p_des=0; |
shaorui | 51:adc72e125b15 | 655 | // controller.v_des = 1.5f; //GR=49 |
shaorui | 51:adc72e125b15 | 656 | controller.v_des = 0.8f; //GR=89 |
shaorui | 48:1b51771c3647 | 657 | controller.kp = 0; |
shaorui | 48:1b51771c3647 | 658 | controller.kd = 5.0f; |
shaorui | 48:1b51771c3647 | 659 | controller.t_ff = 0; |
shaorui | 48:1b51771c3647 | 660 | wait(.5); |
shaorui | 48:1b51771c3647 | 661 | // } |
shaorui | 48:1b51771c3647 | 662 | /* |
shaorui | 48:1b51771c3647 | 663 | else |
shaorui | 48:1b51771c3647 | 664 | { |
Rushu | 49:7eac11914980 | 665 | test_jointround_flag=0; |
shaorui | 48:1b51771c3647 | 666 | controller.v_des =0; |
shaorui | 48:1b51771c3647 | 667 | |
shaorui | 48:1b51771c3647 | 668 | } |
shaorui | 48:1b51771c3647 | 669 | */ |
shaorui | 48:1b51771c3647 | 670 | } |
Rushu | 49:7eac11914980 | 671 | else if(controller.theta_joint_raw*57.2957795>=(359)) |
shaorui | 48:1b51771c3647 | 672 | { |
shaorui | 48:1b51771c3647 | 673 | //stop_sign=1; |
shaorui | 48:1b51771c3647 | 674 | controller.v_des = 0; |
shaorui | 48:1b51771c3647 | 675 | wait(1); |
shaorui | 48:1b51771c3647 | 676 | controller.p_des=0; |
shaorui | 51:adc72e125b15 | 677 | //controller.v_des = -1.5f;//GR=49 |
shaorui | 51:adc72e125b15 | 678 | controller.v_des = -0.8f; //GR=89 |
shaorui | 48:1b51771c3647 | 679 | controller.kp = 0; |
shaorui | 48:1b51771c3647 | 680 | controller.kd = 5.0f; |
shaorui | 48:1b51771c3647 | 681 | controller.t_ff = 0; |
shaorui | 48:1b51771c3647 | 682 | wait(.5); |
shaorui | 48:1b51771c3647 | 683 | printf("test position:%.3f\n\r",(1.0f/GR)* spi.GetMechPosition()); |
shaorui | 48:1b51771c3647 | 684 | |
shaorui | 48:1b51771c3647 | 685 | } |
shaorui | 48:1b51771c3647 | 686 | } |
Rushu | 49:7eac11914980 | 687 | //wait(.1); |
shaorui | 48:1b51771c3647 | 688 | // printf("%.3f\n\r",(1.0f/GR)* spi.GetMechPosition()); |
Rushu | 49:7eac11914980 | 689 | // printf("%.3d, %.3d\n\r",test_jointround_flag, stop_sign); |
shaorui | 48:1b51771c3647 | 690 | |
shaorui | 48:1b51771c3647 | 691 | //printf("BCT: %.3x zzz: %.3x etxy: %.3x \n\r",ma700.Gettest(),ma700.Gettest1(),ma700.Gettest2()); |
shaorui | 48:1b51771c3647 | 692 | // float joint_mech_position=-(controller.theta_mech*360/(2.0f*PI)*GR+controller.theta_mech1*360/(2.0f*PI)); |
shaorui | 48:1b51771c3647 | 693 | // wucha1=(controller.theta_mech-controller.theta_mech1)*360/(2.0f*PI); |
shaorui | 48:1b51771c3647 | 694 | //wucha1=controller.theta_mech*360/(2.0f*PI)-joint_mech_position; |
shaorui | 48:1b51771c3647 | 695 | //wucha+=abs(wucha1); |
shaorui | 48:1b51771c3647 | 696 | //printf("M: %.3f J: %.3f E: %.3f EA: %.3f \n\r",controller.theta_mech*360/(2.0f*PI),controller.theta_mech1*360/(2.0f*PI),wucha1,float(wucha/i)) ; |
shaorui | 48:1b51771c3647 | 697 | // printf("M: %.3f J: %.3f E: %.3f EA: %.3f \n\r",controller.theta_mech*360/(2.0f*PI),joint_mech_position,wucha1,float(wucha/i)) ; |
shaorui | 48:1b51771c3647 | 698 | //printf("m_position: %.3f\n\r",controller.theta_mech*360/(2.0f*PI)*GR); |
shaorui | 48:1b51771c3647 | 699 | //printf("j_position: %.3f\n\r",controller.theta_mech1*360/(2.0f*PI)); |
Rushu | 49:7eac11914980 | 700 | // float m_position=controller.theta_mech*57.2957795; |
shaorui | 48:1b51771c3647 | 701 | // float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR; |
Rushu | 49:7eac11914980 | 702 | // float j_position=-(controller.theta_mech1+controller.theta_mech)*2807.49319614; |
shaorui | 48:1b51771c3647 | 703 | // float j_position=-controller.theta_mech1*57.2957795; |
Rushu | 49:7eac11914980 | 704 | //printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position); |
Rushu | 49:7eac11914980 | 705 | if(count >= 4000){ |
shaorui | 51:adc72e125b15 | 706 | // printf("J: %.3f Mec: %.3f Jerr: %.3f JVerr: %.3f Kp: %.3f Kd: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd); |
shaorui | 51:adc72e125b15 | 707 | //printf("J: %.3f Mec: %.3f J-Mec: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.theta_joint - controller.theta_mech)*57.2957795); |
shaorui | 51:adc72e125b15 | 708 | // printf("J: %.3f Mec: %.3f J-Real: %.3f J-Mec: %.3f Jinit: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, spi.GetMechPosition()*57.2957795/GR,(controller.theta_joint - controller.theta_mech)*57.2957795,(controller.theta_joint- (1.0f/GR)*spi.GetMechPosition())*57.2957795); |
shaorui | 51:adc72e125b15 | 709 | // printf("Ele-Real:%.3f \n\rJ-Des: %.3f \n\r J-Real: %.3f \n\r Des-Real: %.3f \n\r",spi.GetMechPosition()*57.2957795,spi.GetMechPosition()*57.2957795/GR, controller.theta_joint_raw*57.2957795, spi.GetMechPosition()*57.2957795/GR-controller.theta_joint_raw*57.2957795); |
shaorui | 51:adc72e125b15 | 710 | printf("J: %.3f Mec: %.3f J-Mec: %.3f J-Real: %.3f J-Ncycle: %.3d NCycle: %.3d mode-error: %.3f J-M-flag: %.3d J-init: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, |
shaorui | 51:adc72e125b15 | 711 | (controller.theta_joint - controller.theta_mech)*57.2957795,controller.theta_joint_raw*57.2957795,controller.Ncycle,NCycle,abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR),J_M_flag,Joint_init*57.2957795); |
shaorui | 51:adc72e125b15 | 712 | |
Rushu | 50:1fe5c2c53af1 | 713 | // printf("Pdes: %.3f Vdes: %.3f Kp: %.3f Kd: %.3f Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff); |
Rushu | 50:1fe5c2c53af1 | 714 | // printf("Prel: %.3f Vrel: %.3f T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT); |
Rushu | 49:7eac11914980 | 715 | count = 0; |
shaorui | 51:adc72e125b15 | 716 | |
shaorui | 51:adc72e125b15 | 717 | printf("input: %.3f output: %.3f -: %.3f\n\r",(1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_joint_raw*57.2957795,((1.0f/GR)*spi.GetMechPosition()*57.2957795-controller.theta_joint_raw*57.2957795)); |
shaorui | 51:adc72e125b15 | 718 | |
Rushu | 49:7eac11914980 | 719 | } |
Rushu | 49:7eac11914980 | 720 | |
shaorui | 48:1b51771c3647 | 721 | |
Rushu | 49:7eac11914980 | 722 | i++; |
Rushu | 49:7eac11914980 | 723 | //wait(.5); |
shaorui | 48:1b51771c3647 | 724 | |
shaorui | 48:1b51771c3647 | 725 | } |
shaorui | 48:1b51771c3647 | 726 | |
shaorui | 48:1b51771c3647 | 727 | |
benkatz | 0:4e1c4df6aabd | 728 | } |
benkatz | 0:4e1c4df6aabd | 729 | } |