File content as of revision 3:4f35942aee8e:

#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
Timer t;


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


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

// Drive Motor Pins
DigitalOut bl_ddir(p20);
PwmOut bl_pwm(p21);
DigitalOut br_ddir(p19);
PwmOut br_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

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

int main() {
    t.start();
    swerve.begin();
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;
    pc.baud(9600);
    bluetooth.baud(9600);
    
       

    if (pc.writeable()) {
        pc.putc('s'); 
    }
    
    uint32_t prev_time = 0;
    uint32_t interval = .1*1000000;
    
    int counter = 0;
    while(1) {
        curr_time = t.read_us();
//        swerve.update(curr_time);
//        swerve.drive(0.1f, 0.1f, 0.1f);
        if ((curr_time - prev_time) > interval) {
            myled4 = !myled4;
            prev_time = curr_time;
        }
        if (pc.readable()) {
            while (pc.getc() != 'a') {
                }
            for (int ind = 0; ind < CTRL_CNT; ind++) {
                float f;
                char b[4];
                for (int byte_index = 0; byte_index < 4; byte_index++) {
                    b[byte_index] = pc.getc(); 
                }
                memcpy(&f, &b, sizeof(f));
                controller[ind] = f;
            }
        }

 
        myled1 = abs(controller[0]);
        myled2 = abs(controller[1]);
        myled3 = abs(controller[2]);
    }
    
}