vanwall.c

/*
======= Vanwall Library Code =======
- Vanwall is a VEX Robotics library intended to streamline the process of programming VEX Robots
- It provides intuitive interfaces for working with Motors and Controllers
*/

/* Motor Types, for the _type member of Motor structs */
typedef enum {HIGH_SPEED, TORQUE} MotorType;

/* Coefficient Presets, for the _coeff member of Motor structs */
const float STANDARD =  1.0;
const float FLIPPED  = -1.0;
const float HALVED   =  0.5;

/* Create the Motor Struct */
typedef struct motor_struct{
    MotorType     _type;     // The type of the motor - HIGH_SPEED, TORQUE etc.
    tMotor        _port;     // The motor's port, NOT an integer but a RobotC type - use values port1 ... port10
    float         _coeff;    // Coefficient to be applied to speed changes for motor, not inherited by bound motors
    int           _speed;
    void*         _bound;
    bool          _virtual;
} Motor;

void create_motor(Motor *m, MotorType type, tMotor port, float coeff){
     m->_type    = type;
     m->_port    = port;
     m->_coeff   = coeff;
     m->_speed   = 0;
     m->_bound   = NULL;
     m->_virtual = false;
}

void create_virtual_motor(Motor *m, MotorType type, tMotor port, float coeff){
    m->_type    = type;
    m->_port    = port;
    m->_coeff   = coeff;
    m->_speed   = 0;
    m->_bound   = NULL;
    m->_virtual = true;
}

int  get_motor_speed(Motor *m){
    return m->_speed;
}

MotorType get_motor_type(Motor *m){
    return m->_type;
}

tMotor get_motor_port(Motor *m){
	  return m->_port;
}

int  get_motor_coefficient(Motor *m){
	  return m->_coeff;
}

Motor* get_bound_motor(Motor *m){
	  return m->_bound;
}

void set_bound_motor(Motor *m, Motor *slave){
    m->_bound = slave;
}

void del_bound_motor(Motor *m){
	m->_bound = NULL;
}

bool is_virtual(Motor *m){
	return m->_virtual;
}

void set_motor_speed(Motor *m, int speed){
    m->_speed = speed;

	  if (!is_virtual(m)){
	      int adj_speed = round(speed * get_motor_coefficient(m));
	      motor[m->_port] = adj_speed;
    }
    else {
        char buf[64];
    	  char fmt[] = "Motor %d -> %d [Coefficient: %f]";
    	  sprintf(buf, fmt, get_motor_port(m), get_motor_speed(m), get_motor_coefficient(m));
        writeDebugStreamLine(buf);
    }

    if (m->_bound != NULL){
    	set_motor_speed(m->_bound, speed);
    }
}

/* Drive Configurations */

struct LinearFourDrive {
	  Motor* front_left;
	  Motor* front_right;
	  Motor* rear_left;
	  Motor* rear_right;
	  int    _lspeed;
	  int    _rspeed;
}

void create_linear_four(LinearFourDrive *lfd, Motor *fl, Motor *fr, Motor *rl, Motor *rr){
    set_bound_motor(fl, rl);
    set_bound_motor(fr, rr);

    lfd->front_left    = fl;
    lfd->front_right   = fr;
    lfd->rear_left     = rl;
    lfd->rear_right    = rr;

    lfd->_lspeed       = 0;
    lfd->_rspeed       = 0;
}

void set_left_speed_linear_four(LinearFourDrive *lfd, int speed){
    set_motor_speed(lfd->front_left, speed);
    lfd->_lspeed = speed;
}

void set_right_speed_linear_four(LinearFourDrive *lfd, int speed){
    set_motor_speed(lfd->front_right, speed);
    lfd->_rspeed = speed;
}

int get_left_speed_linear_four(LinearFourDrive *lfd){
	  return lfd->_lspeed;
}

int get_right_speed_linear_four(LinearFourDrive *lfd){
 	  return lfd->_rspeed;
}

// Update from controls

int joystick(TVexJoysticks chan){
	  return vexRT[chan];
}

void control_motor_digital(Motor *m, int pchan, int nchan, int speed){
    set_motor_speed(m, speed * (pchan - nchan));
}

void control_linear_four_tank(LinearFourDrive *lfd, int lchan, int rchan){
        set_left_speed_linear_four  (lfd, lchan);
        set_right_speed_linear_four (lfd, rchan);
}

void control_linear_four_arcade(LinearFourDrive *lfd, int powerchan, int rotchan){
        int rotation = rotchan;
        if (abs(rotation) > 10){
            set_left_speed_linear_four(lfd, rotation);
            set_right_speed_linear_four(lfd, -rotation);
        }
        else {
          	int power = powerchan;
            set_left_speed_linear_four  (lfd, power);
            set_right_speed_linear_four (lfd, power);
        }
}

// LCD Utilities

void lcd_display_linear_four(LinearFourDrive *lfd){
	    char topbuf[16];
	    char btmbuf[16];
	  	sprintf(topbuf, "%d     %d", get_motor_speed(lfd->front_left), get_motor_speed(lfd->front_right));
	  	sprintf(btmbuf, "%d     %d", get_motor_speed(lfd->rear_left) , get_motor_speed(lfd->rear_right));
	  	displayLCDCenteredString(0, topbuf);
	  	displayLCDCenteredString(1, btmbuf);
}