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

Timer t;


// 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);


// 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);

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

uint32_t curr_time = 0; // In microseconds
 

int main() {   

    t.start();
    bl_steer.zero();
    bl_steer.zero();
    bl_steer.zero();
    bl_steer.zero();
    
    while(1){
        curr_time = t.read_us();
        bl_steer.update(curr_time);
        br_steer.update(curr_time);
        fl_steer.update(curr_time);
        fr_steer.update(curr_time);
        
        myled1 = bl_steer.is_on_target();
        myled2 = br_steer.is_on_target();
        myled3 = fl_steer.is_on_target();
        myled4 = fr_steer.is_on_target();
        
        if (curr_time < 3000000){
            bl_steer.set_angle(90.0);
            br_steer.set_angle(90.0);
            fl_steer.set_angle(90.0);
            fr_steer.set_angle(90.0);
        }
        else if (curr_time < 6000000){
            bl_steer.set_angle(30.0);
            br_steer.set_angle(30.0);
            fl_steer.set_angle(30.0);
            fr_steer.set_angle(30.0);
        }
        else if (curr_time < 9000000){
            bl_steer.set_angle(120.0);
            br_steer.set_angle(120.0);
            fl_steer.set_angle(120.0);
            fr_steer.set_angle(120.0);
        }
        else if (curr_time < 12000000){
            bl_steer.set_angle(0.0);
            br_steer.set_angle(0.0);
            fl_steer.set_angle(0.0);
            fr_steer.set_angle(0.0);
        }
    }
}

 