void vect_normalize (Vector v)
{
float mag = vect_mag (v);
if (mag > 0.0)
vect_scale (v, v, 1.0/mag);
}
/* Return the angle between two vectors */
float vect_angle (Vector v1, Vector v2)
{
float mag1, mag2, angle, cos_theta;
mag1 = vect_mag(v1);
mag2 = vect_mag(v2);
if (mag1 * mag2 == 0.0)
angle = 0.0;
else {
cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
if (cos_theta <= -1.0)
angle = 180.0;
else if (cos_theta >= +1.0)
angle = 0.0;
else
angle = (180.0/PI) * acos(cos_theta);
}
return angle;
}
void* swerveSyncFunc(void* args) {
float deadZone = 0.05;
while(true) {
int enc_ref_l = safe_enc(swerve_enc_fl->Get());
int enc_ref_r = enc_ref_l; //temp until control is ready
if (driveRun) {
if (fabs(joystick->GetRawAxis(JOY_AXIS_LX)) > deadZone || fabs(joystick->GetRawAxis(JOY_AXIS_LY)) > deadZone) {
//use arithmetic on enc_ref in case both sticks are in use
double rad = atan2(joystick->GetRawAxis(JOY_AXIS_LX), -joystick->GetRawAxis(JOY_AXIS_LY));
double deg = rad * (180 / M_PI);
if (deg < 0) {
deg += 360;
}
enc_ref_l = deg_to_enc(deg);
enc_ref_r = deg_to_enc(deg);
}
SmartDashboard::PutString("DB/String 3", std::to_string(joystick->GetRawAxis(JOY_AXIS_RX)));
if (fabs(joystick->GetRawAxis(JOY_AXIS_RX)) > deadZone) {
int target_l = 1000 - (joystick->GetRawAxis(JOY_AXIS_RX) * 200);
int target_r = 1000 + (joystick->GetRawAxis(JOY_AXIS_RX) * 200);
swerve_mov_fl->SetRaw(target_l);
swerve_mov_fr->SetRaw(target_r);
swerve_mov_bl->SetRaw(target_l);
swerve_mov_br->SetRaw(target_r);
}
}
SmartDashboard::PutString("DB/String 2", std::to_string(enc_ref_l));
SmartDashboard::PutString("DB/String 7", std::to_string(enc_ref_r));
rot_from_enc(swerve_enc_fl, swerve_rot_fl, enc_ref_l);
rot_from_enc(swerve_enc_fr, swerve_rot_fr, enc_ref_r);
rot_from_enc(swerve_enc_bl, swerve_rot_bl, enc_ref_l);
rot_from_enc(swerve_enc_br, swerve_rot_br, enc_ref_r);
float mag = vect_mag(joystick->GetRawAxis(JOY_AXIS_LX), joystick->GetRawAxis(JOY_AXIS_LY));
int target = 1000 - (mag * 100);
swerve_mov_fl->SetRaw(target);
swerve_mov_fr->SetRaw(target);
swerve_mov_bl->SetRaw(target);
swerve_mov_br->SetRaw(target);
}
}
/*
Decodes a 3x4 transformation matrix into separate scale, rotation,
translation, and shear vectors. Based on a program by Spencer W.
Thomas (Graphics Gems II)
*/
void mat_decode (Matrix mat, Vector scale, Vector shear, Vector rotate,
Vector transl)
{
int i;
Vector row[3], temp;
for (i = 0; i < 3; i++)
transl[i] = mat[3][i];
for (i = 0; i < 3; i++) {
row[i][X] = mat[i][0];
row[i][Y] = mat[i][1];
row[i][Z] = mat[i][2];
}
scale[X] = vect_mag (row[0]);
vect_normalize (row[0]);
shear[X] = vect_dot (row[0], row[1]);
row[1][X] = row[1][X] - shear[X]*row[0][X];
row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
scale[Y] = vect_mag (row[1]);
vect_normalize (row[1]);
if (scale[Y] != 0.0)
shear[X] /= scale[Y];
shear[Y] = vect_dot (row[0], row[2]);
row[2][X] = row[2][X] - shear[Y]*row[0][X];
row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
shear[Z] = vect_dot (row[1], row[2]);
row[2][X] = row[2][X] - shear[Z]*row[1][X];
row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
scale[Z] = vect_mag (row[2]);
vect_normalize (row[2]);
if (scale[Z] != 0.0) {
shear[Y] /= scale[Z];
shear[Z] /= scale[Z];
}
vect_cross (temp, row[1], row[2]);
if (vect_dot (row[0], temp) < 0.0) {
for (i = 0; i < 3; i++) {
scale[i] *= -1.0;
row[i][X] *= -1.0;
row[i][Y] *= -1.0;
row[i][Z] *= -1.0;
}
}
if (row[0][Z] < -1.0) row[0][Z] = -1.0;
if (row[0][Z] > +1.0) row[0][Z] = +1.0;
rotate[Y] = asin(-row[0][Z]);
if (fabs(cos(rotate[Y])) > EPSILON) {
rotate[X] = atan2 (row[1][Z], row[2][Z]);
rotate[Z] = atan2 (row[0][Y], row[0][X]);
}
else {
rotate[X] = atan2 (row[1][X], row[1][Y]);
rotate[Z] = 0.0;
}
/* Convert the rotations to degrees */
rotate[X] = (180.0/PI)*rotate[X];
rotate[Y] = (180.0/PI)*rotate[Y];
rotate[Z] = (180.0/PI)*rotate[Z];
}
