File content as of revision 3:f68eaa68f4ec:

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

// 500 x 3 array of degree values
const float trajectory[1000][3] = {
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0,
49,49,0
};

//control flow
volatile int getting_up = 1; //are we currently in the process of standing up
volatile int getting_down = 0; //are we currently in the process of sitting down

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

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 = 6000; //mA

const float time_out_current = 4000; //mA if avg_current is above, increment the timeout count
const int time_out_steps = 5; //max steps after which kill the test
volatile int time_out_count[3] = {0,0,0};  //counter for time out
int kill_test = 0; //kill switch

// 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() {
    if(getting_up){
        rear_n_d = -50*(.001*current_sample)*n2d; //linear ramp up over 500 samples
        front_n_d = -50*(.001*current_sample)*n2d; //linear ramp up over 500 samples
        spine_n_d = 0.0; 
    }
    else if(getting_down){
        rear_n_d = -50*(1-.001*current_sample)*n2d; //linear ramp up over 500 samples
        front_n_d = -50*(1-.001*current_sample)*n2d; //linear ramp up over 500 samples
        spine_n_d = 0.0; 
    }
    else{
        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); 

    //timeout for motor safety
    if( avg_current[rear] > time_out_current){ time_out_count[rear]++;}
    if( avg_current[front] > time_out_current){ time_out_count[front]++;}
    if( avg_current[spine] > time_out_current){ time_out_count[spine]++;}
    if( time_out_count[rear]>time_out_steps){ fclose(fp);kill_test = 1;}
    if( time_out_count[front]>time_out_steps){ fclose(fp);kill_test = 1;}
    if( time_out_count[spine]>time_out_steps){ fclose(fp);kill_test = 1;}



    //step to next control point
    if (getting_up){
        if(current_sample == 1000){ getting_up = 0; current_sample = 0;} //we're up
        else{  current_sample++;} //still getting up
    }
    else if(getting_down){
        if(current_sample == 1000){ //we're down
            tick.detach();
            tock.detach();
            fclose(fp);
            pwm = 0;
            updateMotor(rear,pwm); 
            updateMotor(front,pwm);
            updateMotor(spine,pwm);
        }
        else{current_sample++;}    //still getting down
    }
    else if (current_sample == n_samples){ //normal operation
        if (current_loop == n_loops){ getting_down = 1; current_sample=0;} //ready to sit
        else{ //end of loop, ready for next
            current_sample = 0;
            current_loop++;
        }
    }
    else{ current_sample++;} //middle of running loop
} 

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(~kill_test) {
        //DEBUG
//        pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
    }
}