Created
January 4, 2017 18:16
-
-
Save fourohfour/e0e64566d9dc7c754fb7a63a476b3838 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
======= 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); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment