File content as of revision 2:1ae2ebcba406:

#ifndef SWERVEMODULE_H
#define SWERVEMODULE_H

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

class SwerveModule {
private:
    SteerMotor* _steer;
    DriveMotor* _drive;

public:
    SwerveModule(SteerMotor* steer, DriveMotor* drive) : _steer(steer), _drive(drive) {}
    
    void begin() {
        _steer->zero();
        _drive->kill();
    }
    
    // speed is number from 0-1, angle is in degrees from 0-360
    void drive(float speed, float angle){
        float curr_angle = _steer->get_angle();
        if ((angle > curr_angle + 90.0f && angle < curr_angle + 270.0f) ||
            (angle > curr_angle - 270.0f && angle < curr_angle - 90.0f)){
            angle += (angle < 180.0f) ? 180.0f : -180.0f;
            _drive->set_dir(0);
        } else {
            _drive->set_dir(1);
        }
        if (speed < 0.01){
            _steer->set_angle(curr_angle);
        } else {
            _steer->set_angle(angle);
        }
        _drive->set_speed(speed);
    }
    
    void kill(){
        _steer->kill();
        _drive->kill();
    }
    
    void update(uint32_t curr_time){
        _steer->update(curr_time);
    }
    
    bool is_on_target(){
        return _steer->is_on_target();
    }
    
};

#endif