void mainConvert(void){
yaxis = GetJoystickAnalog( 1 , 3 ) ;
xaxis = GetJoystickAnalog( 1 , 4 ) ;
axaxis = GetJoystickAnalog( 1 , 1 ) ;
ayaxis = GetJoystickAnalog( 1 , 2 ) ;
lt = GetJoystickDigital ( 1 , 5 , 2 ) ;
ls = GetJoystickDigital ( 1 , 5 , 1 ) ;
rt = GetJoystickDigital ( 1 , 6 , 2 ) ;
rs = GetJoystickDigital ( 1 , 6 , 1 ) ;
tu = GetJoystickDigital ( 1 , 7 , 2 ) ;
td = GetJoystickDigital ( 1 , 7 , 1 ) ;
tl = GetJoystickDigital ( 1 , 7 , 3 ) ;
tr = GetJoystickDigital ( 1 , 7 , 4 ) ;
b1 = GetJoystickDigital ( 1 , 8 , 1 ) ;
b2 = GetJoystickDigital ( 1 , 8 , 3 ) ;
b3 = GetJoystickDigital ( 1 , 8 , 2 ) ;
b4 = GetJoystickDigital ( 1 , 8 , 4 ) ;
bateryV = GetMainBattery ( ) ;
rotation = GetQuadEncoder ( 5 , 6 ) ;
gyro = GetGyroAngle ( 1 ) ;
///*
if(cal == 0){
if(gyro < 1408){
SetMotor ( 6 , 120 ) ;
Wait ( 1600 ) ;
SetMotor ( 6 , 20 ) ;
}
cal = 1;
scal = 1;
}
if(scal == 1){
StartQuadEncoder ( 5 , 6 , 0 ) ;
scal =0;
}
//*/
if(b4 == 1){
shift = 3;
}else{
shift = 1;
}
if(bateryV >= 7.5){
SetDigitalOutput ( 10 , 1 ) ;
}
SetDigitalOutput ( 12 , 0 ) ;
//dis = GetUltrasonic ( 8 , 9 ) ;
//PrintToScreen ( "%d\n" , dis ) ;
//PrintToScreen ( "%d\n" , gyro ) ;
//PrintToScreen ( "%d\n" , rotation) ;
//Wait ( 500 ) ;
if(axaxis <= 20 && axaxis >= -20){
Arcade4 ( 1 , 3 , 4 , 2 , 3 , 4 , 5 , 0 , 0 , 0 , 0 ) ;
SetDigitalOutput ( 12 , 1 ) ;
}
if(axaxis >= 20 || axaxis <= -20){
SetDigitalOutput ( 12 , 1 ) ;
SetMotor ( 2 , axaxis ) ;
SetMotor ( 3 , axaxis ) ;
SetMotor ( 4 , axaxis * -1) ;
SetMotor ( 5 , axaxis * -1) ;
// input1 + input2 = xplimit
// imput1 - input2 = xnlimit
}
if(rs == 1){
SetMotor ( 6 , 127/shift) ;
//gyroP = gyro;
rotationP = rotation;
}
else if(rt == 1){
SetMotor ( 6 , -127/shift) ;
//gyroP = gyro;
rotationP = rotation;
}
else{
SetMotor ( 6 , 0) ;
}
if(ls == 1){
SetMotor ( 7 , 127) ;
close = 0;
}
else if(lt == 1){
SetMotor ( 7 , -127) ;
close = 1;
}
else if(close == 1){
SetMotor ( 7 , -50) ;
}else{
SetMotor ( 7 , 0) ;
}
if(rs == 0 && rt == 0){
if(rotation < rotationP){
SetMotor ( 6 , -20) ;
}else if(rotation > rotationP){
SetMotor ( 6 , 20) ;
}
}
}
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the oversampling rate, the
* gyro type and the A/D calibration values.
* The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
*
* @param channel The analog channel the gyro is plugged into
* @return the current heading of the robot in degrees. This heading is based on integration
* of the returned rate from the gyro.
*/
float GetGyroAngle(UINT32 channel)
{
return GetGyroAngle(SensorBase::GetDefaultAnalogModule(), channel);
}
void Drive::Actuate(){
flPID->SetSource( ( 1 / flEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
blPID->SetSource( -( 1 / blEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
frPID->SetSource( -( 1 / frEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
brPID->SetSource( ( 1 / brEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
double flVel = 1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
double blVel = 1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
double frVel = -1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
double brVel = -1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
/*xPID->SetSource( flVel + brVel - blVel - frVel );
yPID->SetSource( flVel + brVel + blVel + frVel );
turnPID->SetSource( flVel - brVel + blVel - frVel );*/
double x = driverX;
double y = driverY;
double turn = driverTurn;
if( isFieldOriented ){
double gAngle = GetGyroAngle();
double cosA = cos( gAngle * 3.14159 / 180 );
double sinA = sin( gAngle * 3.14159 / 180 );
x = driverX*cosA - driverY*sinA;
y = driverX*sinA + driverY*cosA;
}
if( isHoldAngle ){
double gyroAngle = fmod( GetGyroAngle(), 360.0 );
double relativeAngle = gyroAngle - targetAngle;
if( relativeAngle > 180 ){
relativeAngle = ( 180 - ( relativeAngle - 180 ) );
}
turn = Drive::ANGLE_P * relativeAngle;
}
double fl = ( y + x + turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
double bl = ( y - x + turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
double fr = ( y - x - turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
double br = ( y + x - turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
if( isPIDControl ){
flPID->SetSetpoint( fl, fl );
blPID->SetSetpoint( bl, bl );
frPID->SetSetpoint( fr, fr );
brPID->SetSetpoint( br, br );
fl = flPID->GetOutput();
bl = blPID->GetOutput();
fr = frPID->GetOutput();
br = brPID->GetOutput();
//fl = xPID->GetOutput() + yPID->GetOutput() + turnPID->GetOutput() ;
//bl = -xPID->GetOutput() + yPID->GetOutput() + turnPID->GetOutput() ;
//fr = -xPID->GetOutput() + yPID->GetOutput() - turnPID->GetOutput() ;
//br = xPID->GetOutput() + yPID->GetOutput() - turnPID->GetOutput() ;
}
flMotor->Set( fl * motorInverters[0] );
blMotor->Set( bl * motorInverters[1] );
frMotor->Set( fr * motorInverters[2] );
brMotor->Set( br * motorInverters[3] );
}
