Shao Rui / Joint_control_2AS5147_DRV8323RH_GR49_HJB_2020_8_10

1

Dependencies:   mbed-dev-f303 FastPWM3

main.cpp@9:d7eb815cb057, 2016-05-10 (annotated)

Committer:
benkatz
Date:
Tue May 10 01:15:57 2016 +0000
Revision:
9:d7eb815cb057
Parent:
8:10ae7bc88d6e
Child:
10:370851e6e132
Fixed current scaling hack

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