File content as of revision 4:7be1da65326d:

#include "mbed.h"
#include "SteerMotor.h"
#include "DriveMotor.h"
#include "SwerveModule.h"
#include "SwerveDrive.h"

#define CTRL_CNT 3   // number of 4 byte packages sent over bluetooth
#define MAX_SPEED 0.25

Timer t;


// UART
Serial  pc(USBTX, USBRX);
RawSerial   esp8266(p13,p14);


// Steer Motor Pins
DigitalOut bl_sdir(p10);
DigitalOut bl_step(p9);
DigitalOut br_sdir(p12);
DigitalOut br_step(p11);
DigitalOut fl_sdir(p8);
DigitalOut fl_step(p7);
DigitalOut fr_sdir(p6);
DigitalOut fr_step(p5);

// Drive Motor Pins
DigitalOut br_ddir(p20);
PwmOut br_pwm(p21);
DigitalOut bl_ddir(p19);
PwmOut bl_pwm(p22);
DigitalOut fl_ddir(p18);
PwmOut fl_pwm(p23);
DigitalOut fr_ddir(p17);
PwmOut fr_pwm(p24);

// Motors
SteerMotor bl_steer(bl_sdir, bl_step);
SteerMotor br_steer(br_sdir, br_step);
SteerMotor fl_steer(fl_sdir, fl_step);
SteerMotor fr_steer(fr_sdir, fr_step);
DriveMotor bl_drive(bl_ddir, bl_pwm);
DriveMotor br_drive(br_ddir, br_pwm);
DriveMotor fl_drive(fl_ddir, fl_pwm);
DriveMotor fr_drive(fr_ddir, fr_pwm);

// Modules
SwerveModule bl_module(&bl_steer, &bl_drive);
SwerveModule br_module(&br_steer, &br_drive);
SwerveModule fl_module(&fl_steer, &fl_drive);
SwerveModule fr_module(&fr_steer, &fr_drive);

SwerveDrive swerve(&fl_module, &fr_module, &bl_module, &br_module);

PwmOut myled1(LED1);
PwmOut myled2(LED2);
PwmOut myled3(LED3);
PwmOut myled4(LED4);

uint32_t curr_time = 0; // In microseconds
uint32_t prev_time = 0; // Also in microseconds
uint8_t ind = 0;
uint8_t byte_index = 0;
float f;
char b[4];

//l_joy_x, l_joy_y, r_joy_x
float controller[CTRL_CNT] = {0};
 

int main() {   
    t.start();
    swerve.begin();
    swerve.kill();
    
    fr_drive.set_multiplier(1.00 * MAX_SPEED);
    fl_drive.set_multiplier(0.68 * MAX_SPEED);
    bl_drive.set_multiplier(0.56 * MAX_SPEED);
    br_drive.set_multiplier(0.77 * MAX_SPEED);
    
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;
    pc.baud(9600);
    esp8266.baud(9600);
    
    while(1) {
        curr_time = t.read_us();
        
        swerve.update(curr_time);
        swerve.drive(controller[1], controller[0], controller[2]);
        
        if (esp8266.readable()) {
            char temp = esp8266.getc();
            if (byte_index > 3) { 
                memcpy(&f, &b, sizeof(f));
                controller[ind] = f;
                byte_index = 0;
                ind = ind == 2 ? 0 : ind + 1;
            }
            if (temp == 'a'){
                prev_time = t.read_us();
                byte_index = 0;
                ind = 0;
            } else {
                b[byte_index] = temp;
                byte_index++;
            }
        } else {
            if (curr_time - prev_time > 100000){
                myled4 = 0;
                for (int ind = 0; ind < CTRL_CNT; ind++) {
                    controller[ind] = 0;
                }
                swerve.kill();
            }
        }
 
        myled1 = abs(controller[0]);
        myled2 = abs(controller[1]);
        myled3 = abs(controller[2]);
    }
}