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] ); }