File content as of revision 2:17379e2a6f7d:

#include "mbed.h"
#include "QEI.h"
#define CONTROL_PERIOD 0.002 // 500Hz ***
#define SAVE_PERIOD 0.005 // 200HZ

//think about start up and shut down sequences

//control flow
volatile int current_sample = 0;
volatile int current_loop = 0;
const int n_samples = 1000;
const int n_loops = 2;

// 500 x 3 array of degree values
const int trajectory[1000][3] = {
1,1,0,
1,1,0,
1,1,0,
1,1,0,
1,1,0,
1,1,0,
1,1,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,1,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,2,
0,0,3,
0,0,3,
0,0,3,
0,0,3,
0,0,3,
0,0,3,
0,0,3,
0,0,3,
0,0,3,
0,0,4,
0,0,4,
0,0,4,
0,0,4,
0,0,4,
0,0,4,
0,0,4,
0,0,4,
0,0,5,
0,0,5,
0,0,5,
0,0,5,
0,0,5,
0,0,5,
0,0,5,
0,0,5,
0,0,6,
0,0,6,
0,0,6,
0,0,6,
0,0,6,
0,0,6,
0,0,6,
0,0,6,
0,0,6,
0,0,7,
0,0,7,
0,0,7,
0,0,7,
0,0,7,
0,0,7,
0,0,7,
0,0,7,
0,0,8,
0,0,8,
0,0,8,
0,0,8,
0,0,8,
0,0,8,
0,0,8,
0,0,8,
0,0,8,
0,0,9,
0,0,9,
0,0,9,
0,0,9,
0,0,9,
0,0,9,
0,0,9,
0,0,9,
0,0,10,
0,0,10,
0,0,10,
0,0,10,
0,0,10,
0,0,10,
0,0,10,
0,0,10,
0,0,11,
0,0,11,
0,0,11,
0,0,11,
0,0,11,
0,0,11,
0,0,11,
0,0,11,
0,0,11,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,12,
0,0,13,
0,0,13,
0,0,13,
0,0,13,
0,0,13,
0,0,13,
0,0,13,
0,0,13,
0,0,13,
1,1,13,
1,1,13,
1,1,13,
1,1,13,
1,1,14,
1,1,14,
1,1,14,
1,1,14,
1,1,14,
1,1,14,
2,2,14,
2,2,14,
2,2,14,
2,2,14,
2,2,14,
2,2,14,
2,2,14,
2,2,14,
3,3,14,
3,3,14,
3,3,14,
3,3,14,
3,3,14,
3,3,14,
4,4,14,
4,4,14,
4,4,14,
4,4,14,
4,4,14,
4,4,14,
5,5,14,
5,5,14,
5,5,14,
5,5,14,
5,5,14,
6,6,14,
6,6,14,
6,6,14,
6,6,14,
6,6,14,
7,7,14,
7,7,14,
7,7,14,
7,7,14,
7,7,14,
8,8,14,
8,8,14,
8,8,14,
8,8,14,
8,8,14,
9,9,14,
9,9,14,
9,9,14,
9,9,14,
9,9,15,
10,10,15,
10,10,15,
10,10,15,
10,10,15,
10,10,15,
11,11,15,
11,11,15,
11,11,15,
11,11,15,
11,11,15,
12,12,15,
12,12,15,
12,12,15,
12,12,15,
12,12,15,
13,13,15,
13,13,15,
13,13,15,
13,13,15,
13,13,15,
14,14,15,
14,14,15,
14,14,15,
14,14,15,
14,14,15,
15,15,15,
15,15,15,
15,15,14,
15,15,14,
15,15,14,
16,16,14,
16,16,14,
16,16,14,
16,16,14,
16,16,14,
17,17,14,
17,17,14,
17,17,14,
17,17,14,
17,17,14,
18,18,14,
18,18,14,
18,18,14,
18,18,14,
18,18,14,
19,19,14,
19,19,14,
19,19,14,
19,19,14,
19,19,14,
20,20,14,
20,20,14,
20,20,14,
20,20,14,
20,20,14,
21,21,14,
21,21,14,
21,21,14,
21,21,14,
21,21,14,
21,21,14,
22,22,14,
22,22,14,
22,22,14,
22,22,14,
23,23,14,
23,23,14,
23,23,14,
23,23,14,
23,23,14,
24,24,14,
24,24,14,
24,24,14,
24,24,14,
24,24,14,
25,25,14,
25,25,14,
25,25,13,
25,25,13,
25,25,13,
26,26,13,
26,26,13,
26,26,13,
26,26,13,
26,26,13,
27,27,13,
27,27,13,
27,27,13,
27,27,13,
27,27,13,
28,28,12,
28,28,12,
28,28,12,
28,28,12,
28,28,12,
29,29,12,
29,29,12,
29,29,12,
29,29,12,
29,29,12,
30,30,11,
30,30,11,
30,30,11,
30,30,11,
30,30,11,
31,31,11,
31,31,11,
31,31,11,
31,31,11,
31,31,10,
32,32,10,
32,32,10,
32,32,10,
32,32,10,
32,32,10,
33,33,10,
33,33,10,
33,33,9,
33,33,9,
33,33,9,
33,33,9,
34,34,9,
34,34,9,
34,34,9,
34,34,9,
34,34,9,
35,35,8,
35,35,8,
35,35,8,
35,35,8,
36,36,8,
36,36,8,
36,36,8,
36,36,8,
36,36,7,
37,37,7,
37,37,7,
37,37,7,
37,37,7,
37,37,7,
38,38,7,
38,38,7,
38,38,6,
38,38,6,
38,38,6,
39,39,6,
39,39,6,
39,39,6,
39,39,6,
39,39,6,
40,40,5,
40,40,5,
40,40,5,
40,40,5,
40,40,5,
40,40,5,
41,41,5,
41,41,5,
41,41,5,
41,41,4,
41,41,4,
42,42,4,
42,42,4,
42,42,4,
42,42,4,
42,42,4,
43,43,4,
43,43,3,
43,43,3,
43,43,3,
43,43,3,
44,44,3,
44,44,3,
44,44,3,
44,44,3,
44,44,2,
45,45,2,
45,45,2,
45,45,2,
45,45,2,
45,45,2,
45,45,2,
46,46,2,
46,46,2,
46,46,1,
46,46,1,
46,46,1,
46,46,1,
47,47,1,
47,47,1,
47,47,1,
47,47,1,
47,47,0,
47,47,0,
47,47,0,
47,47,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
48,48,0,
49,49,0,
49,49,0,
49,49,0,
49,49,-1,
49,49,-1,
49,49,-1,
49,49,-1,
49,49,-1,
49,49,-1,
49,49,-1,
49,49,-1,
49,49,-2,
49,49,-2,
49,49,-2,
49,49,-2,
49,49,-2,
49,49,-2,
49,49,-2,
49,49,-2,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-3,
49,49,-4,
49,49,-4,
49,49,-4,
49,49,-4,
49,49,-4,
49,49,-4,
49,49,-4,
49,49,-4,
49,49,-5,
49,49,-5,
49,49,-5,
49,49,-5,
49,49,-5,
49,49,-5,
49,49,-5,
50,50,-5,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-6,
50,50,-7,
50,50,-7,
50,50,-7,
50,50,-7,
50,50,-7,
50,50,-7,
50,50,-7,
50,50,-7,
50,50,-8,
50,50,-8,
50,50,-8,
50,50,-8,
50,50,-8,
50,50,-8,
50,50,-8,
50,50,-8,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-9,
50,50,-10,
50,50,-10,
50,50,-10,
50,50,-10,
50,50,-10,
50,50,-10,
50,50,-10,
50,50,-10,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-11,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-12,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-13,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-14,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
50,50,-15,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
49,49,-14,
48,48,-14,
48,48,-14,
48,48,-14,
48,48,-14,
48,48,-14,
48,48,-14,
48,48,-14,
48,48,-13,
48,48,-13,
48,48,-13,
47,47,-13,
47,47,-13,
47,47,-13,
47,47,-13,
47,47,-13,
47,47,-13,
47,47,-13,
47,47,-13,
46,46,-13,
46,46,-13,
46,46,-12,
46,46,-12,
46,46,-12,
46,46,-12,
45,45,-12,
45,45,-12,
45,45,-12,
45,45,-12,
45,45,-12,
45,45,-12,
44,44,-11,
44,44,-11,
44,44,-11,
44,44,-11,
44,44,-11,
43,43,-11,
43,43,-11,
43,43,-11,
43,43,-11,
43,43,-10,
42,42,-10,
42,42,-10,
42,42,-10,
42,42,-10,
42,42,-10,
41,41,-10,
41,41,-10,
41,41,-9,
41,41,-9,
41,41,-9,
40,40,-9,
40,40,-9,
40,40,-9,
40,40,-9,
40,40,-9,
39,39,-8,
39,39,-8,
39,39,-8,
39,39,-8,
39,39,-8,
38,38,-8,
38,38,-8,
38,38,-8,
38,38,-8,
38,38,-7,
37,37,-7,
37,37,-7,
37,37,-7,
37,37,-7,
37,37,-7,
36,36,-7,
36,36,-7,
36,36,-6,
36,36,-6,
36,36,-6,
36,36,-6,
35,35,-6,
35,35,-6,
35,35,-6,
35,35,-6,
34,34,-5,
34,34,-5,
34,34,-5,
34,34,-5,
34,34,-5,
34,34,-5,
33,33,-5,
33,33,-5,
33,33,-5,
33,33,-4,
33,33,-4,
32,32,-4,
32,32,-4,
32,32,-4,
32,32,-4,
31,31,-4,
31,31,-4,
31,31,-3,
31,31,-3,
31,31,-3,
30,30,-3,
30,30,-3,
30,30,-3,
30,30,-3,
30,30,-3,
30,30,-3,
29,29,-2,
29,29,-2,
29,29,-2,
29,29,-2,
28,28,-2,
28,28,-2,
28,28,-2,
28,28,-2,
28,28,-2,
28,28,-2,
27,27,-1,
27,27,-1,
27,27,-1,
27,27,-1,
27,27,-1,
26,26,-1,
26,26,-1,
26,26,-1,
26,26,-1,
26,26,-1,
25,25,-1,
25,25,-1,
25,25,-1,
25,25,0,
25,25,0,
24,24,0,
24,24,0,
24,24,0,
24,24,0,
24,24,0,
23,23,0,
23,23,0,
23,23,0,
23,23,0,
23,23,0,
22,22,0,
22,22,0,
22,22,0,
22,22,0,
21,21,0,
21,21,0,
21,21,0,
21,21,0,
21,21,0,
20,20,0,
20,20,0,
20,20,0,
20,20,0,
20,20,0,
19,19,0,
19,19,0,
19,19,0,
19,19,0,
19,19,0,
18,18,0,
18,18,0,
18,18,0,
18,18,0,
18,18,0,
17,17,0,
17,17,0,
17,17,0,
17,17,0,
17,17,0,
16,16,0,
16,16,0,
16,16,0,
16,16,0,
16,16,0,
15,15,0,
15,15,0,
15,15,0,
15,15,0,
15,15,0,
14,14,0,
14,14,0,
14,14,0,
14,14,0,
14,14,0,
13,13,0,
13,13,0,
13,13,0,
13,13,0,
13,13,0,
12,12,0,
12,12,0,
12,12,0,
12,12,0,
12,12,0,
11,11,0,
11,11,0,
11,11,0,
11,11,0,
11,11,0,
10,10,0,
10,10,0,
10,10,0,
10,10,0,
10,10,0,
9,9,0,
9,9,0,
9,9,0,
9,9,0,
9,9,0,
9,9,0,
8,8,0,
8,8,0,
8,8,0,
8,8,0,
8,8,0,
7,7,0,
7,7,0,
7,7,0,
7,7,0,
7,7,0,
6,6,0,
6,6,0,
6,6,0,
6,6,0,
6,6,0,
5,5,0,
5,5,0,
5,5,0,
5,5,0,
5,5,0,
4,4,0,
4,4,0,
4,4,0,
4,4,0,
4,4,0,
4,4,0,
3,3,0,
3,3,0,
3,3,0,
3,3,0,
3,3,0,
3,3,0,
2,2,0,
2,2,0,
2,2,0,
2,2,0,
2,2,0,
2,2,0,
2,2,0,
2,2,0,
1,1,0,
1,1,0,
1,1,0
};

Ticker tick;
Ticker tock;
Timer t;

Serial pc(USBTX, USBRX); // tx, rx
LocalFileSystem local("data");               // Create the local filesystem under the name "local"

// Declare Three Encoders
QEI rear_encoder(p22, p23, NC, 1200, QEI::X4_ENCODING);  // rear leg
QEI front_encoder(p5, p6, NC, 1200, QEI::X4_ENCODING);  // front leg
QEI spine_encoder(p9, p10, NC, 1200, QEI::X4_ENCODING);  // spine

// Specify pinout
DigitalOut  rear_motorA(p15);
DigitalOut  rear_motorB(p16);
PwmOut      rear_motorPWM(p24);
AnalogIn    rear_cs(p20);

DigitalOut  front_motorA(p7);
DigitalOut  front_motorB(p8);
PwmOut      front_motorPWM(p25);
AnalogIn    front_cs(p19);

DigitalOut  spine_motorA(p11);
DigitalOut  spine_motorB(p12);
PwmOut      spine_motorPWM(p26);
AnalogIn    spine_cs(p18);

//LEDs for current safety
DigitalOut rear_led(LED1);
DigitalOut front_led(LED2);
DigitalOut spine_led(LED3);

//number domains for abstraction
const int rear = 0;
const int front = 1;
const int spine = 2;

// Sensing Variables
volatile int i = 0;
volatile float w = 0;
volatile int id = 4000;
volatile int sign = 0;

volatile int n[3] = {0,0,0}; //encoder values
volatile int last_n[3] = {0,0,0}; //previous encoder values

volatile float avg_current[3] = {0,0,0}; //integration of current in time

// Output Variables
volatile float pwm = 0;

// Control Constants
const float R = 2.3/1000.0; // [kohm]
const float Kv = 0.1682;
const int Vs = 18; // [V]
const float n2d = 3.3333;

const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
const float stall_current = 8000; //mA

// Control Parameters
float rear_Kp = 0.001;
float rear_Ks_p = 250.0;
float rear_Ks_d = 25.0;

float front_Kp = 0.001;
float front_Ks_p = 250.0;
float front_Ks_d = 25.0;

float spine_Kp = 0.001;
float spine_Ks_p = 200.0;
float spine_Ks_d = 20.0;

float rear_n_d = 0.0*n2d;
float front_n_d = 0.0*n2d;
float spine_n_d = 0.0*n2d;
float rear_w_d = 0;
float front_w_d = 0;
float spine_w_d = 0;


FILE *fp = fopen("/data/out.txt", "w");  // Open "out.txt" on the local file system for writing

int read_current(int which_domain) {
    int current = 0;
    if (which_domain == rear) {                // rear
        current = rear_cs.read()*23570;
    } else if (which_domain == front) {         // front
        current = front_cs.read()*23570;
    } else if (which_domain == spine){          // spine
        current = spine_cs.read()*23570;
    }
    avg_current[which_domain] = (1-integ_alpha)*avg_current[which_domain] + integ_alpha*current;  //integrate
    return current; //mA
}

void updateMotor(int which_motor, float power) {
    int dir = 0;
    
    if (power < 0) { 
        power = -power; 
        dir = 0;
    } else {
        dir = 1;
    }
    if (power > 1) { power = 1; }
    if (power < 0) { power = 0; }
    
    if (which_motor == rear) {                 // rear
        if (dir == 1) {
            rear_motorA = 0;
            rear_motorB = 1;
        } else {
            rear_motorA = 1;
            rear_motorB = 0;
        }
        rear_motorPWM.write(power);
    } else if (which_motor == front) {          // front
        if (dir == 1) {
            front_motorA = 0;
            front_motorB = 1;
        } else {
            front_motorA = 1;
            front_motorB = 0;
        }
        front_motorPWM.write(power);
    } else if (which_motor == spine) {          // spine
        if (dir == 1) {
            spine_motorA = 0;
            spine_motorB = 1;
        } else {
            spine_motorA = 1;
            spine_motorB = 0;
        }
        spine_motorPWM.write(power);
    }
}

float updateEncoder(int which_encoder) {
    last_n[which_encoder] = n[which_encoder];
    if(which_encoder == rear){
        n[which_encoder] = rear_encoder.getPulses();
    }
    else if(which_encoder == front){
        n[which_encoder] = front_encoder.getPulses();
    }
    else if(which_encoder == spine){
        n[which_encoder] = spine_encoder.getPulses();
    }
    w = (n[which_encoder]-last_n[which_encoder])*1.047;       //steps/s --> rad/s
    return w;
 }

void control() {
    rear_n_d = -trajectory[current_sample][rear]*n2d;
    front_n_d = -trajectory[current_sample][front]*n2d;
    spine_n_d = -trajectory[current_sample][spine]*n2d;

    // rear
    i = read_current(rear);
    w = updateEncoder(rear);      
    id = rear_Ks_p*(rear_n_d-n[rear]) + rear_Ks_d*(rear_w_d-w);
    sign = abs(id)/id;
    id = abs(id);
    pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
    if (avg_current[rear] > stall_current){pwm = 0;rear_led=1;}
    updateMotor(rear,pwm); 
    
    // front
    i = read_current(front);
    w = updateEncoder(front);      
    id = front_Ks_p*(front_n_d-n[front]) + front_Ks_d*(front_w_d-w);
    sign = abs(id)/id;
    id = abs(id);
    pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
    if (avg_current[front] > stall_current){pwm = 0;front_led=1;}
    updateMotor(front,pwm); 
    
    // spine
    i = read_current(spine);
    w = updateEncoder(spine);
    id = spine_Ks_p*(spine_n_d-n[spine]) + spine_Ks_d*(spine_w_d-w);
    sign = abs(id)/id;
    id = abs(id);
    pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
    if (avg_current[spine] > stall_current){pwm = 0;spine_led=1;}
    updateMotor(spine,pwm); 

    //step to next control point
    if (current_sample == n_samples){
        if (current_loop == n_loops){ //done
            tick.detach();
            tock.detach();
            fclose(fp);
            pwm = 0;
            updateMotor(rear,pwm); 
            updateMotor(front,pwm);
            updateMotor(spine,pwm);
        }
        else{ //end of loop, ready for next
            current_sample = 0;
            current_loop++;
        }
    }
    else{ //middle of loop
        current_sample++;
    }
} 

void save() {
    fprintf(fp, "%i %i %i %i %f %f %f\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2]);
}

int main() {
    rear_motorPWM.period(0.00005);   //20kHz
    front_motorPWM.period(0.00005);   //20kHz
    spine_motorPWM.period(0.00005);   //20kHz
    tick.attach(&control,CONTROL_PERIOD);
    tock.attach(&save,SAVE_PERIOD);
    t.start();
    
    while(1) { 
        //DEBUG
//        pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
    }
}