Shao Rui / test_motor_output_position

1

Dependencies:   mbed-dev-f303 FastPWM3

main.cpp@10:370851e6e132, 2016-05-12 (annotated)

Committer:
benkatz
Date:
Thu May 12 05:02:52 2016 +0000
Revision:
10:370851e6e132
Parent:
9:d7eb815cb057
Child:
11:c83b18d41e54
Now featuring hobbyking startup tone!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:4e1c4df6aabd 1 #include "mbed.h"
benkatz 0:4e1c4df6aabd 2 #include "PositionSensor.h"
benkatz 0:4e1c4df6aabd 3 #include "Inverter.h"
benkatz 0:4e1c4df6aabd 4 #include "SVM.h"
benkatz 0:4e1c4df6aabd 5 #include "FastMath.h"
benkatz 0:4e1c4df6aabd 6 #include "Transforms.h"
benkatz 0:4e1c4df6aabd 7 #include "CurrentRegulator.h"
benkatz 8:10ae7bc88d6e 8 #include "TorqueController.h"
benkatz 9:d7eb815cb057 9 #include "ImpedanceController.h"
benkatz 9:d7eb815cb057 10
benkatz 9:d7eb815cb057 11 ///SPI Input Stuff
benkatz 9:d7eb815cb057 12 //DigitalIn cselect(PB_12);
benkatz 9:d7eb815cb057 13 //InterruptIn select(PB_12);
benkatz 9:d7eb815cb057 14 //DigitalIn mosi(PB_15);
benkatz 9:d7eb815cb057 15 //SPISlave input(PB_15, PB_14, PB_13, PB_12);
benkatz 9:d7eb815cb057 16
benkatz 9:d7eb815cb057 17 int id[3] = {0};
benkatz 9:d7eb815cb057 18 float cmd_float[3] = {0.0f};
benkatz 9:d7eb815cb057 19 int raw[3] = {0};
benkatz 9:d7eb815cb057 20 float val_max[3] = {18.0f, 1.0f, 0.1f};
benkatz 9:d7eb815cb057 21 int buff[8];
benkatz 9:d7eb815cb057 22 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 23
benkatz 8:10ae7bc88d6e 24 //PositionSensorEncoder encoder(8192,4.0f);
benkatz 2:8724412ad628 25 //Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
benkatz 9:d7eb815cb057 26 Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005); //hall motter
benkatz 8:10ae7bc88d6e 27 //Inverter inverter(PA_10, PA_9, PA_8, PB_7, 0.01007080078, 0.00005); //test motter
benkatz 10:370851e6e132 28 //PositionSensorSPI spi(2048, 2.75f, 7); ///1 I really need an eeprom or something to store this....
benkatz 10:370851e6e132 29 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
benkatz 10:370851e6e132 30 PositionSensorSPI spi(2048, 3.0, 21);
benkatz 9:d7eb815cb057 31 int motorID = 40; ///1
benkatz 9:d7eb815cb057 32 //int motorID = 50; ///2
benkatz 9:d7eb815cb057 33
benkatz 10:370851e6e132 34 //PositionSensorEncoder encoder(1024, 0, 7);
benkatz 10:370851e6e132 35 PositionSensorEncoder encoder(1024, 0, 21);
benkatz 10:370851e6e132 36
benkatz 8:10ae7bc88d6e 37
benkatz 8:10ae7bc88d6e 38 CurrentRegulator foc(&inverter, &spi, .005, .5); //hall sensor
benkatz 9:d7eb815cb057 39 TorqueController torqueController(.031f, &foc);
benkatz 9:d7eb815cb057 40 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
benkatz 9:d7eb815cb057 41
benkatz 8:10ae7bc88d6e 42 //CurrentRegulator foc(&inverter, &encoder, .005, .5); //test motter
benkatz 9:d7eb815cb057 43 //SVPWM svpwm(&inverter, 2.0f);
benkatz 8:10ae7bc88d6e 44
benkatz 7:dc5f27756e02 45 Ticker testing;
benkatz 7:dc5f27756e02 46 //Timer t;
benkatz 0:4e1c4df6aabd 47
benkatz 8:10ae7bc88d6e 48 /*
benkatz 7:dc5f27756e02 49 float v_d = 0;
benkatz 7:dc5f27756e02 50 float v_q = .1;
benkatz 7:dc5f27756e02 51 float v_alpha = 0;
benkatz 7:dc5f27756e02 52 float v_beta = 0;
benkatz 7:dc5f27756e02 53 float v_a = 0;
benkatz 7:dc5f27756e02 54 float v_b = 0;
benkatz 7:dc5f27756e02 55 float v_c = 0;
benkatz 8:10ae7bc88d6e 56 */
benkatz 9:d7eb815cb057 57 float ref = 0.0;
benkatz 9:d7eb815cb057 58 int count = 0;
benkatz 4:c023f7b6f462 59
benkatz 4:c023f7b6f462 60 //SPI spi(PB_15, PB_14, PB_13);
benkatz 4:c023f7b6f462 61 //GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
benkatz 4:c023f7b6f462 62
benkatz 4:c023f7b6f462 63 //DigitalOut chipselect(PB_12);
benkatz 4:c023f7b6f462 64
benkatz 0:4e1c4df6aabd 65 using namespace FastMath;
benkatz 0:4e1c4df6aabd 66 using namespace Transforms;
benkatz 0:4e1c4df6aabd 67
benkatz 0:4e1c4df6aabd 68
benkatz 1:b8bceb4daed5 69 // Current Sampling IRQ
benkatz 2:8724412ad628 70 /*
benkatz 0:4e1c4df6aabd 71 extern "C" void TIM2_IRQHandler(void) {
benkatz 0:4e1c4df6aabd 72 // flash on update event
benkatz 0:4e1c4df6aabd 73 if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
benkatz 0:4e1c4df6aabd 74 inverter.SampleCurrent();
benkatz 0:4e1c4df6aabd 75 }
benkatz 0:4e1c4df6aabd 76 TIM2->SR = 0x0; // reset the status register
benkatz 0:4e1c4df6aabd 77 }
benkatz 2:8724412ad628 78 */
benkatz 8:10ae7bc88d6e 79
benkatz 8:10ae7bc88d6e 80
benkatz 8:10ae7bc88d6e 81
benkatz 2:8724412ad628 82 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 83 // toggle on update event
benkatz 2:8724412ad628 84 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 85 inverter.SampleCurrent();
benkatz 7:dc5f27756e02 86 //wait(.00002);
benkatz 9:d7eb815cb057 87 //foc.Commutate(); ///Putting the loop here doesn't work for some reason. Need to figure out why
benkatz 2:8724412ad628 88 }
benkatz 2:8724412ad628 89 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 90 //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
benkatz 2:8724412ad628 91 }
benkatz 0:4e1c4df6aabd 92
benkatz 8:10ae7bc88d6e 93
benkatz 10:370851e6e132 94 void hk_start(void){
benkatz 10:370851e6e132 95 inverter.SetDTC(0, 0, 0);
benkatz 10:370851e6e132 96 inverter.EnableInverter();
benkatz 10:370851e6e132 97 for(int i = 0; i<120; i++){
benkatz 10:370851e6e132 98 torqueController.SetTorque(.4);
benkatz 10:370851e6e132 99 wait(0.000956);
benkatz 10:370851e6e132 100 torqueController.SetTorque(-.4);
benkatz 10:370851e6e132 101 wait(0.000956);
benkatz 10:370851e6e132 102 }
benkatz 10:370851e6e132 103 for(int i = 0; i<120; i++){
benkatz 10:370851e6e132 104 torqueController.SetTorque(.4);
benkatz 10:370851e6e132 105 wait(0.0008513);
benkatz 10:370851e6e132 106 torqueController.SetTorque(-.4);
benkatz 10:370851e6e132 107 wait(0.0008513);
benkatz 10:370851e6e132 108 }
benkatz 10:370851e6e132 109 for(int i = 0; i<120; i++){
benkatz 10:370851e6e132 110 torqueController.SetTorque(.4);
benkatz 10:370851e6e132 111 wait(0.00075843);
benkatz 10:370851e6e132 112 torqueController.SetTorque(-.4);
benkatz 10:370851e6e132 113 wait(0.00075843);
benkatz 10:370851e6e132 114 }
benkatz 10:370851e6e132 115 torqueController.SetTorque(0);
benkatz 10:370851e6e132 116 wait(.4);
benkatz 10:370851e6e132 117 for (int j = 0; j<3; j++){
benkatz 10:370851e6e132 118 for(int i = 0; i<120; i++){
benkatz 10:370851e6e132 119 torqueController.SetTorque(.4);
benkatz 10:370851e6e132 120 wait(0.000956);
benkatz 10:370851e6e132 121 torqueController.SetTorque(-.4);
benkatz 10:370851e6e132 122 wait(0.000956);
benkatz 10:370851e6e132 123 }
benkatz 10:370851e6e132 124 torqueController.SetTorque(0);
benkatz 10:370851e6e132 125 wait(.2);
benkatz 10:370851e6e132 126
benkatz 10:370851e6e132 127 }
benkatz 10:370851e6e132 128
benkatz 10:370851e6e132 129 }
benkatz 10:370851e6e132 130
benkatz 8:10ae7bc88d6e 131
benkatz 8:10ae7bc88d6e 132 /*
benkatz 7:dc5f27756e02 133 void voltage_foc(void){
benkatz 7:dc5f27756e02 134 float theta = encoder.GetElecPosition();
benkatz 7:dc5f27756e02 135 InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
benkatz 7:dc5f27756e02 136 InvClarke(v_alpha, v_beta, &v_a, &v_b, &v_c);
benkatz 7:dc5f27756e02 137 svpwm.Update_DTC(v_a, v_b, v_c);
benkatz 7:dc5f27756e02 138 //output.write(theta/6.28318530718f);
benkatz 7:dc5f27756e02 139 }
benkatz 8:10ae7bc88d6e 140 */
benkatz 9:d7eb815cb057 141 /*
benkatz 9:d7eb815cb057 142 void read(void){
benkatz 9:d7eb815cb057 143 int startByte;
benkatz 9:d7eb815cb057 144 if(input.receive()){
benkatz 9:d7eb815cb057 145 //startByte = input.read();
benkatz 9:d7eb815cb057 146 //if(startByte == 65535){
benkatz 9:d7eb815cb057 147 //startByte = input.read();
benkatz 9:d7eb815cb057 148 //wait(.000005);
benkatz 9:d7eb815cb057 149 raw[0] = input.read();
benkatz 9:d7eb815cb057 150 raw[1] = input.read();
benkatz 9:d7eb815cb057 151 raw[2] = input.read();
benkatz 9:d7eb815cb057 152 id[0] = raw[0]>>14;
benkatz 9:d7eb815cb057 153 id[1] = raw[1]>>14;
benkatz 9:d7eb815cb057 154 id[2] = raw[2]>>14;
benkatz 9:d7eb815cb057 155 printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
benkatz 9:d7eb815cb057 156 for(int i = 0; i<3; i++){
benkatz 9:d7eb815cb057 157 cmd_float[id[i]] = (val_max[id[i]])*(float)(raw[i] - (id[i]<<14))/16383.0f;
benkatz 9:d7eb815cb057 158 }
benkatz 9:d7eb815cb057 159 // }
benkatz 9:d7eb815cb057 160 // else{
benkatz 9:d7eb815cb057 161 // input.read();
benkatz 9:d7eb815cb057 162 // input.read();
benkatz 9:d7eb815cb057 163 // input.read();
benkatz 9:d7eb815cb057 164 // }
benkatz 9:d7eb815cb057 165 //printf("%d %d %d \n\r", raw[0], raw[1], raw[2]);
benkatz 9:d7eb815cb057 166 }
benkatz 9:d7eb815cb057 167
benkatz 9:d7eb815cb057 168 }
benkatz 9:d7eb815cb057 169
benkatz 9:d7eb815cb057 170 */
benkatz 9:d7eb815cb057 171
benkatz 9:d7eb815cb057 172 void serialInterrupt(void){
benkatz 9:d7eb815cb057 173 //wait(.001);
benkatz 9:d7eb815cb057 174 int i = 0;
benkatz 9:d7eb815cb057 175 while(pc.readable()){
benkatz 9:d7eb815cb057 176 buff[i] = pc.getc();
benkatz 9:d7eb815cb057 177 wait(.0001);
benkatz 9:d7eb815cb057 178 i++;
benkatz 9:d7eb815cb057 179
benkatz 9:d7eb815cb057 180 }
benkatz 9:d7eb815cb057 181 int val = (buff[4]<<8) + buff[5];
benkatz 9:d7eb815cb057 182 int checksum = buff[2]^buff[3]^buff[4]^buff[5];
benkatz 9:d7eb815cb057 183 int validStart = (buff[0] == 255 && buff[1] == 255 && buff[2]==motorID && checksum==buff[6]);
benkatz 9:d7eb815cb057 184
benkatz 9:d7eb815cb057 185 if(validStart){
benkatz 9:d7eb815cb057 186
benkatz 9:d7eb815cb057 187 switch(buff[3]){
benkatz 9:d7eb815cb057 188 case 10:
benkatz 9:d7eb815cb057 189 cmd_float[1] = (float)val*val_max[1]/65278.0f;
benkatz 9:d7eb815cb057 190 break;
benkatz 9:d7eb815cb057 191 case 20:
benkatz 9:d7eb815cb057 192 cmd_float[2] = (float)val*val_max[2]/65278.0f;
benkatz 9:d7eb815cb057 193 break;
benkatz 9:d7eb815cb057 194 case 30:
benkatz 9:d7eb815cb057 195 cmd_float[0] = (float)val*val_max[0]/65278.0f;
benkatz 9:d7eb815cb057 196 break;
benkatz 9:d7eb815cb057 197 }
benkatz 9:d7eb815cb057 198 }
benkatz 9:d7eb815cb057 199
benkatz 9:d7eb815cb057 200
benkatz 9:d7eb815cb057 201 //pc.printf("%d %d %d %d %d %d %d \n", start1, start2, id, cmd, byte1, byte2, byte3);
benkatz 9:d7eb815cb057 202 //pc.printf("%f, %f, %f\n", cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 203 //pc.printf("%d\n", cmd);
benkatz 9:d7eb815cb057 204 //pc.printf("%d, %d, %d, %d, %d, %d, %d, %d\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6], buff[7]);
benkatz 9:d7eb815cb057 205 }
benkatz 9:d7eb815cb057 206
benkatz 0:4e1c4df6aabd 207 void Loop(void){
benkatz 9:d7eb815cb057 208
benkatz 10:370851e6e132 209 //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
benkatz 10:370851e6e132 210 impedanceController.SetImpedance(-.04, -0.00, 0);
benkatz 9:d7eb815cb057 211
benkatz 9:d7eb815cb057 212 count = count+1;
benkatz 9:d7eb815cb057 213
benkatz 9:d7eb815cb057 214 if(count > 1000){
benkatz 9:d7eb815cb057 215 //ref= -1*ref;
benkatz 9:d7eb815cb057 216 //printf("%f %f %f \n\r", cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 217 //float e = spi.GetElecPosition();
benkatz 9:d7eb815cb057 218 //printf("%f\n\r", e);
benkatz 9:d7eb815cb057 219 count = 0;
benkatz 9:d7eb815cb057 220 }
benkatz 9:d7eb815cb057 221
benkatz 9:d7eb815cb057 222
benkatz 9:d7eb815cb057 223 //torqueController.SetTorque(0);
benkatz 9:d7eb815cb057 224 //foc.Commutate();
benkatz 7:dc5f27756e02 225 //voltage_foc();
benkatz 0:4e1c4df6aabd 226 }
benkatz 3:6a0015d88d06 227
benkatz 3:6a0015d88d06 228 void PrintStuff(void){
benkatz 9:d7eb815cb057 229 //float v = encoder.GetMechVelocity();
benkatz 8:10ae7bc88d6e 230 //float position = encoder.GetElecPosition();
benkatz 8:10ae7bc88d6e 231 //float position = encoder.GetMechPosition();
benkatz 9:d7eb815cb057 232 //float m = spi.GetMechPosition();
benkatz 10:370851e6e132 233 //float e = spi.GetElecPosition();
benkatz 10:370851e6e132 234 //printf("%f\n\r", e);
benkatz 9:d7eb815cb057 235 //printf("%f %f %f %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 236 //printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
benkatz 3:6a0015d88d06 237 }
benkatz 7:dc5f27756e02 238
benkatz 7:dc5f27756e02 239
benkatz 1:b8bceb4daed5 240
benkatz 7:dc5f27756e02 241
benkatz 7:dc5f27756e02 242 /*
benkatz 7:dc5f27756e02 243 void gen_sine(void){
benkatz 7:dc5f27756e02 244 float f = 1.0f;
benkatz 7:dc5f27756e02 245 float time = t.read();
benkatz 7:dc5f27756e02 246 float a = .45f*sin(6.28318530718f*f*time) + .5f;
benkatz 7:dc5f27756e02 247 float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
benkatz 7:dc5f27756e02 248 float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
benkatz 7:dc5f27756e02 249 inverter.SetDTC(a, b, c);
benkatz 7:dc5f27756e02 250 }
benkatz 7:dc5f27756e02 251 */
benkatz 0:4e1c4df6aabd 252
benkatz 0:4e1c4df6aabd 253 int main() {
benkatz 9:d7eb815cb057 254 //mosi.mode(PullDown);
benkatz 9:d7eb815cb057 255 //cselect.mode(PullUp);
benkatz 9:d7eb815cb057 256 inverter.DisableInverter();
benkatz 9:d7eb815cb057 257 spi.ZeroPosition();
benkatz 9:d7eb815cb057 258 //input.format(16, 0);
benkatz 9:d7eb815cb057 259 //input.frequency(100000);
benkatz 9:d7eb815cb057 260 //select.fall(&read);
benkatz 9:d7eb815cb057 261
benkatz 9:d7eb815cb057 262
benkatz 9:d7eb815cb057 263 //NVIC_SetPriority(EXTI15_10_IRQn, 1);
benkatz 9:d7eb815cb057 264 wait(.1);
benkatz 9:d7eb815cb057 265 inverter.SetDTC(0.2, 0.2, 0.2);
benkatz 9:d7eb815cb057 266 inverter.EnableInverter();
benkatz 10:370851e6e132 267 hk_start();
benkatz 10:370851e6e132 268 //foc.Reset();
benkatz 0:4e1c4df6aabd 269 testing.attach(&Loop, .0001);
benkatz 9:d7eb815cb057 270 NVIC_SetPriority(TIM5_IRQn, 2);
benkatz 9:d7eb815cb057 271 pc.baud(115200);
benkatz 9:d7eb815cb057 272 //pc.attach(&serialInterrupt);
benkatz 9:d7eb815cb057 273 //printf("hello\n\r");
benkatz 7:dc5f27756e02 274 //testing.attach(&gen_sine, .01);
benkatz 7:dc5f27756e02 275 //testing.attach(&PrintStuff, .1);
benkatz 8:10ae7bc88d6e 276 //inverter.SetDTC(.05, 0, 0);
benkatz 9:d7eb815cb057 277 //inverter.DisableInverter();
benkatz 7:dc5f27756e02 278 //foc.Commutate();
benkatz 9:d7eb815cb057 279 wait(.5);
benkatz 0:4e1c4df6aabd 280 while(1) {
benkatz 3:6a0015d88d06 281 //printf("%f\n\r", encoder.GetElecPosition());
benkatz 3:6a0015d88d06 282 //wait(.1);
benkatz 0:4e1c4df6aabd 283 }
benkatz 0:4e1c4df6aabd 284 }