Exemple #1
0
void main_process()
{
    print_debug_inf_before_launch();
    DefineRobotMotors(2, 1, 3, 2, 7, 6, 0, 0, 0, 0);
    StartTimer(6);
    while(1) // main loop(joystick)
    {
        PresetTimer(6, 0);
        prepare_gd();
        print_gd_debug_inf();
        Arcade4(1, 3, 1, 3, 2, 7, 6, 0, 0, 0, 0);
        JoystickDigitalToMotorAndLimit(1, 5, 1, 127, 0, 2, -127, 0, 8);
        JoystickDigitalToMotorAndLimit(1, 5, 1, -127, 0, 2, 127, 0, 9);
        JoystickDigitalToMotorAndLimit(1, 6, 1, -127, 0, 2, 127, 0, 4);
        JoystickDigitalToMotorAndLimit(1, 7, 1, -127, 0, 2, 127, 0, 5);
        PrintTextToGD ( 22 , 3 , 0 , "Loop time: %4d\n" , GetTimer(6) ) ;
    }
}
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) ;
}


}





}