void bangTBH ( void )
{
time += 1 ;
encOld = encNew ;
encNew = GetQuadEncoder ( 7 , 8 ) ;
encVelocity = encNew - encOld ;
v1 = v2 ;
v2 = v3 ;
v3 = encVelocity ;
vavg = (v1+v2+v3)/3 ;
current = vavg ;
errorOld = errorNew ;
errorNew = target - current ;
if ( pidOn )
{
command = trueCommand ;
command += errorNew*kbtbh ;
if ( sign(errorNew) != sign(errorOld) )
{
commandTBH = .5 * (commandTBH + command); ;
command = commandTBH ;
}
if ( command > 127 )
{
command = 127 ;
}
if ( command < 0 )
{
command = 0 ;
}
if ( time > 1 && madRush == 0 ) // essentially commented here
{
//madRush = 1 ;
//command = 127 ;
}
trueCommand = command ;
if ( target - vavg > bangLambda )
{
command = 127 ;
bang = 1 ;
}
if ( bang == 1 && target - vavg <= bangLambda )
{
command = trueCommand ;
bang = 0 ;
}
}
if ( Abs(errorOld) < lambda && Abs(errorNew) < lambda )
{
flywheelAtSpeed = 1 ;
}
else
{
flywheelAtSpeed = 0 ;
}
}
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) ;
}
}
}
