motor_ctrl.cpp 13.2 KB
Newer Older
mattdr's avatar
mattdr committed
1
2
3
4
5
6
7
8
9
#include <Arduino.h>
#include "motor_ctrl.h"
#include "utils.h"
#include "encoder.h"
#include "math.h"
#include "sin_factors.h"

#define pi 3.1415926535897932384626433832795

thcl's avatar
thcl committed
10
11
12
#define H1 6 //greenH
#define H2 21  //yellowH
#define H3 23 //blueH
13
#define L1 5 //GreenL                                                                                                                                                                                                                                                                                                               
thcl's avatar
thcl committed
14
15
#define L2 20  //YellowL
#define L3 22 //BlueL
mattdr's avatar
mattdr committed
16

17
18
19
20
#define HB1EN 0 //halfbridge 1 enable
#define HB2EN 1 //halfbridge 2 enable
#define HB3EN 2 //halfbridge 3 enable

21
const int led = 13;
mattdr's avatar
mattdr committed
22
23

// This scale factor scales the potentiometer
24
int const SCALE_FACTOR = 240;
mattdr's avatar
mattdr committed
25
26

// This is the PWM frequency and it is very important
27
int FREQ = 22000; //8000 was our original value;
mattdr's avatar
mattdr committed
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

// This is the lower limit of PWM frequency 
// its important for not breaking gate drivers
float PERIOD = 1/((float) FREQ);
// this helps in calculatations of the lower limit of pwm
float PERIOD_OVER_255 = PERIOD/255.0;





void turn_all_off();
int one_to_scaleFactor(int x);
void set_phase(int high1, int high2, int high3, int low1, int low2, int low3);
void set_block_phase(int phase);
void motor_butt_control_inefficient(void);
void set_phase_float(float f1, float f2, float f3);
void motor_butt_control_precalculated(void);
void charge_bootstrap_capacitors(void);




void motor_setup(void) {
    // put your setup code here, to run once:

    

    pinMode(L1, OUTPUT);
    pinMode(L2, OUTPUT);
    pinMode(L3, OUTPUT); 
    pinMode(H1, OUTPUT);
    pinMode(H2, OUTPUT);
    pinMode(H3, OUTPUT);
    

    //PWM setup
    analogWriteFrequency(H1, FREQ);
    analogWriteFrequency(H2, FREQ);
    analogWriteFrequency(H3, FREQ);

    turn_all_off();
70
71
72
73
74
75
76
77
78
79
80
81

    //setup the halfbridges enables
    pinMode(HB1EN, OUTPUT);
    pinMode(HB2EN, OUTPUT);
    pinMode(HB3EN, OUTPUT);
    //enable the halfbridges
    digitalWrite(HB1EN, HIGH);
    digitalWrite(HB2EN, HIGH);
    digitalWrite(HB3EN, HIGH);

    //setup led
    pinMode(led, OUTPUT);
82
    digitalWrite(led, HIGH);
mattdr's avatar
mattdr committed
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
}

void turn_all_off() {
    digitalWrite(L1, LOW);
    digitalWrite(L2, LOW);
    digitalWrite(L3, LOW);
    analogWrite(H1, 0);
    analogWrite(H2, 0);
    analogWrite(H3, 0);
}

// If this function recives a 1 it will check the potentiometer and
// return a int 0-255 of the value on the pot (scaled)
// if it recives a 0 it will return 0
int one_to_scaleFactor(int x)
{
    if (x == 1)
    {
thcl's avatar
thcl committed
101
        // NOTE: The scale factor is the max value we want out of
mattdr's avatar
mattdr committed
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
        // the potentiometer
        return get_pot_0_scaled(SCALE_FACTOR, PERIOD_OVER_255);
    }
    else{
        return 0;
    }
}

// Sets the phase of the motor where:
/*
1:AB
2:AC
3:BC
4:BA
5:CA
6:CB
ALL OTHER PHASE NUMBERS WILL SET MOTOR TO OFF
*/
void set_block_phase(int phase)
{
    switch(phase) {
        case 1:
            set_phase(1,0,0,0,1,0); //Phase AB
            break;
        case 2:
            set_phase(1,0,0,0,0,1); //Phase AC
            break;
        case 3:
            set_phase(0,1,0,0,0,1); //Phase BC
            break;
        case 4:
            set_phase(0,1,0,1,0,0); //Phase BA
            break;
        case 5:
            set_phase(0,0,1,1,0,0); //Phase CA
            break;
        case 6:
            set_phase(0,0,1,0,1,0); //Phase CB
            break;
        default :
            turn_all_off();
            //no break needed
    }
    return;
}

// This function takes in a seris of 1 and 0s
void set_phase(int high1, int high2, int high3, int low1, int low2, int low3)
{
    high1 = one_to_scaleFactor(high1);
    high2 = one_to_scaleFactor(high2);
    high3 = one_to_scaleFactor(high3);

    //turn the stuff that should be off, off first

    if (! low1) digitalWrite(L1, low1);
158
    if (! low2) digitalWrite(L2, low2);  
mattdr's avatar
mattdr committed
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
    if (! low3) digitalWrite(L3, low3);
    if (! high1) analogWrite(H1, high1);
    if (! high2) analogWrite(H2, high2);
    if (! high3) analogWrite(H3, high3);

    //turn the stuff that should be on, on

    if (low1) digitalWrite(L1, low1);
    if (low2) digitalWrite(L2, low2);
    if (low3) digitalWrite(L3, low3);
    if (high1) analogWrite(H1, high1);
    if (high2) analogWrite(H2, high2);
    if (high3) analogWrite(H3, high3);
}

void set_phase_float(float f1, float f2, float f3)
{
    //deal with the potentiometer first
    // NOTE: The scale factor of is the max value we want out of
    // the potentiometer
179
    //digitalWrite(led, HIGH);// start a timer
mattdr's avatar
mattdr committed
180
    int potval = get_pot_0_scaled(SCALE_FACTOR, PERIOD_OVER_255);
181
182
    //digitalWrite(led, LOW);// stop a timer

mattdr's avatar
mattdr committed
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457

    f1 = (f1 * potval);
    f2 = (f2 * potval);
    f3 = (f3 * potval);

    int f1i = (int) f1;
    int f2i = (int) f2;
    int f3i = (int) f3;

    if(f1i == 0){
        digitalWrite(L1, 1);
    }
    else{
        digitalWrite(L1, 0);
    }
    analogWrite(H1, f1i);

    if(f2i == 0){
        digitalWrite(L2, 1);
    }
    else{
        digitalWrite(L2, 0);
    }
    analogWrite(H2, f2i);

    if(f3i == 0){
        digitalWrite(L3, 1);
    }
    else{
        digitalWrite(L3, 0);
    }
    analogWrite(H3, f3i);
}

void motor_butt_control_inefficient(void)
{
    calibrate_encoder();
    block_until_spacebar();

    while(1)
    {
        int position = get_wheel_position_ticks();
        float n = (position/8192) * 12 * 2 * pi;
        n += 5*(pi/3);
        // n is the mapped position in pi scale
        float f1 = max(0, max(sinf(n),              sinf(n - (pi/3))));
        float f2 = max(0, max(sinf(n + ((2*pi)/3)), sinf(n + (pi/3))));
        float f3 = max(0, max(sinf(n - ((2*pi)/3)), sinf(n - (pi))));

        set_phase_float(f1,f2,f3);
    }
}

void motor_butt_control_precalculated(void)
{
    calibrate_encoder();
    block_until_spacebar();

    while(1)
    {
        // get the current encoder tics (this does the math to offset)
        int position = get_wheel_position_ticks();
        // get position within current electrical revolution
        int ele_position = get_elec_rev_position_ticks(position);
        float f1 = A_SIN_FACTORS_SHIFTED[ele_position];
        float f2 = B_SIN_FACTORS_SHIFTED[ele_position];
        float f3 = C_SIN_FACTORS_SHIFTED[ele_position];

        set_phase_float(f1,f2,f3);

    }
}

//This just spins the motor using speed (potentiometer on pin A1)
//and torque (potentiometer on pin A0)
void open_loop_control(void)
{
    while(1)
    {
        for(int p = 1; p <= 6; p++)
        {
            set_block_phase(p);
            //What's going on with this math, explination:
            // The get_pot_1_inv will return an int 0-100
            // We want the minimum speed to be 1 sec per phase
            // we want the maximum speed to be 2800 us per phase
            // the max speed corresponds to 5 rev/sec 
            // we know we need to go faster but this is just testing
            volatile int myDelay = 1500 + (get_pot_1_inv() * 300);
            // Serial.println("my delay:");  
            // Serial.println(myDelay);  
            // Serial.println("my speed:");  
            // Serial.println(get_pot_1_inv());  
            delayMicroseconds(myDelay);
        }
    }
}

// This just charges up the small boostrap capacitors
// this should allways be called right before powering the motor
void charge_bootstrap_capacitors(void)
{
    turn_all_off();
    delayMicroseconds(50);
    set_phase(0,0,0,1,1,1);
    delayMicroseconds(50);
    turn_all_off();
}

//using hardcoded located state positions
void direct_position_control(void) 
{
    calibrate_encoder();
    //attachInterrupt(digitalPinToInterrupt(7), isr_index_print, RISING);
    block_until_spacebar();

    while(1)
    {
        int e = get_wheel_position_ticks();
        int s = 0;

        if     (e <= 174 ) {s = (1 % 6) + 1;}
        else if(e <= 220 ) {s = (2 % 6) + 1;}
        else if(e <= 397 ) {s = (3 % 6) + 1;}
        else if(e <= 516 ) {s = (4 % 6) + 1;}
        else if(e <= 633 ) {s = (5 % 6) + 1;}
        else if(e <= 687 ) {s = (6 % 6) + 1;}
        else if(e <= 858 ) {s = (1 % 6) + 1;}
        else if(e <= 903 ) {s = (2 % 6) + 1;}
        else if(e <= 1078) {s = (3 % 6) + 1;}
        else if(e <= 1201) {s = (4 % 6) + 1;}
        else if(e <= 1284) {s = (5 % 6) + 1;}
        else if(e <= 1422) {s = (6 % 6) + 1;}
        else if(e <= 1547) {s = (1 % 6) + 1;}
        else if(e <= 1590) {s = (2 % 6) + 1;}
        else if(e <= 1759) {s = (3 % 6) + 1;}
        else if(e <= 1879) {s = (4 % 6) + 1;}
        else if(e <= 1995) {s = (5 % 6) + 1;}
        else if(e <= 2047) {s = (6 % 6) + 1;}
        else if(e <= 2223) {s = (1 % 6) + 1;}
        else if(e <= 2267) {s = (2 % 6) + 1;}
        else if(e <= 2457) {s = (3 % 6) + 1;}
        else if(e <= 2506) {s = (4 % 6) + 1;}
        else if(e <= 2671) {s = (5 % 6) + 1;}
        else if(e <= 2722) {s = (6 % 6) + 1;}
        else if(e <= 2906) {s = (1 % 6) + 1;}
        else if(e <= 2954) {s = (2 % 6) + 1;}
        else if(e <= 3130) {s = (3 % 6) + 1;}
        else if(e <= 3181) {s = (4 % 6) + 1;}
        else if(e <= 3366) {s = (5 % 6) + 1;}
        else if(e <= 3412) {s = (6 % 6) + 1;}
        else if(e <= 3592) {s = (1 % 6) + 1;}
        else if(e <= 3635) {s = (2 % 6) + 1;}
        else if(e <= 3811) {s = (3 % 6) + 1;}
        else if(e <= 3936) {s = (4 % 6) + 1;}
        else if(e <= 3985) {s = (5 % 6) + 1;}
        else if(e <= 4102) {s = (6 % 6) + 1;}
        else if(e <= 4272) {s = (1 % 6) + 1;}
        else if(e <= 4318) {s = (2 % 6) + 1;}
        else if(e <= 4493) {s = (3 % 6) + 1;}
        else if(e <= 4556) {s = (4 % 6) + 1;}
        else if(e <= 4720) {s = (5 % 6) + 1;}
        else if(e <= 4843) {s = (6 % 6) + 1;}
        else if(e <= 4887) {s = (1 % 6) + 1;}
        else if(e <= 5066) {s = (2 % 6) + 1;}
        else if(e <= 5182) {s = (3 % 6) + 1;}
        else if(e <= 5236) {s = (4 % 6) + 1;}
        else if(e <= 5406) {s = (5 % 6) + 1;}
        else if(e <= 5468) {s = (6 % 6) + 1;}
        else if(e <= 5639) {s = (1 % 6) + 1;}
        else if(e <= 5688) {s = (2 % 6) + 1;}
        else if(e <= 5860) {s = (3 % 6) + 1;}
        else if(e <= 5945) {s = (4 % 6) + 1;}
        else if(e <= 6089) {s = (5 % 6) + 1;}
        else if(e <= 6210) {s = (6 % 6) + 1;}
        else if(e <= 6253) {s = (1 % 6) + 1;}
        else if(e <= 6430) {s = (2 % 6) + 1;}
        else if(e <= 6552) {s = (3 % 6) + 1;}
        else if(e <= 6600) {s = (4 % 6) + 1;}
        else if(e <= 6706) {s = (5 % 6) + 1;}
        else if(e <= 6824) {s = (6 % 6) + 1;}
        else if(e <= 6994) {s = (1 % 6) + 1;}
        else if(e <= 7121) {s = (2 % 6) + 1;}
        else if(e <= 7230) {s = (3 % 6) + 1;}
        else if(e <= 7343) {s = (4 % 6) + 1;}
        else if(e <= 7461) {s = (5 % 6) + 1;}
        else if(e <= 7567) {s = (6 % 6) + 1;}
        else if(e <= 7685) {s = (1 % 6) + 1;}
        else if(e <= 7736) {s = (2 % 6) + 1;}
        else if(e <= 7910) {s = (3 % 6) + 1;}
        else if(e <= 8033) {s = (4 % 6) + 1;}
        else if(e <= 8140) {s = (5 % 6) + 1;}
        else if(e <= 8193) {s = (6 % 6) + 1;}


        set_block_phase(s);
    }
}

void debug_motor(void) {
    while(1)
    {
        int i = 0;
        // put your main code here, to run repeatedly:
        Serial.println("I am alive!");   
        short incomingByte;
        delay(50);  // do not print too fast!
        if (Serial.available()) {
            incomingByte = Serial.read();  // will not be -1
            // actually do something with incomingByte
            Serial.println("I got this byte:");   
            Serial.println(incomingByte);   
            
        }
        switch (incomingByte - 48) 
        {
            case 0:
                Serial.println("AROUND THE WORLD");
                for (i = 0; i < 6; i++){
                    set_phase(1,0,0,0,1,0);
                    delay(10);
                    set_phase(1,0,0,0,0,1); 
                    delay(10);
                    set_phase(0,1,0,0,0,1);
                    delay(10);
                    set_phase(0,1,0,1,0,0);
                    delay(10);
                    set_phase(0,0,1,1,0,0);
                    delay(10);
                    set_phase(0,0,1,0,1,0); 
                    delay(10);
                }
                turn_all_off();
                Serial.println("DONE WITH AROUND THE WORLD");
            
            case 1:
                set_phase(1,0,0,0,1,0);
                Serial.println("I AM IN STATE 1-AB");
                break;
            
            case 2:
                set_phase(1,0,0,0,0,1);
                Serial.println("I AM IN STATE 2-AC");
                break;
            
            case 3:
                set_phase(0,1,0,0,0,1);
                Serial.println("I AM IN STATE 3-EC");
                break;
            
            case 4:
                set_phase(0,1,0,1,0,0);
                Serial.println("I AM IN STATE 4-BA");
                break;
            
            case 5:
                set_phase(0,0,1,1,0,0);
                Serial.println("I AM IN STATE 5-CA");
                break;
            
            case 6:
                set_phase(0,0,1,0,1,0);
                Serial.println("I AM IN STATE 6-CB");
                break;
                
            case -16:
                turn_all_off();
                Serial.println("STOP THE PRESSES!!!");
                break;
            
            default:
                break;
        }
    }
}