File content as of revision 2:69d290bd4b7d:

#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() {
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;
    pc.baud(9600);
    bluetooth.baud(9600);
    
       

    //attach interrupt function for each new Bluetooth serial character
//    bluetooth.attach(&parse_message,Serial::RxIrq);
    
    if (pc.writeable()) {
        pc.putc('s'); 
    }
    
    
    int counter = 0;
    while(1) {
//        swerve.update(curr_time);
//        swerve.drive(0.1f, 0.1f, 0.1f);
        myled4 = bluetooth.readable();
        if (bluetooth.readable()) {
            if (bluetooth.getc() == 'a') {
                counter++;
                //receive float (4 bytes)
                
                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] = bluetooth.getc(); 
                        counter++;
                        
                    }
                    memcpy(&f, &b, sizeof(f));
                    controller[ind] = f;
                }
            }
//            while (bluetooth.readable()) {
//                bluetooth.getc();
//            }
            else {
                for (int ind = 0; ind < CTRL_CNT; ind++) {
                    controller[ind] = 0.0;
                }
            }
        }


        pc.printf("%i\r\n", counter);
//        if (abs(controller[0]) > 0.0)
//            myled1 = 1;
//        if (abs(controller[1]) > 0.0)
//            myled2 = 1;
//        if (abs(controller[2]) > 0.0)
//            myled3 = 1;     

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