void ArmResetStateMachine() {
// switch(armResetState) {
// case STOP_PID:
StopIntegratedMotorEncoderPID(armR);
StopIntegratedMotorEncoderPID(armL);
// armResetState = LOWER_ARM;
// break;
// case LOWER_ARM:
while(GetDigitalInput(armResetSwitch)
&& GetJoystickDigital (1, 7, 2)){
SetArm(-75);
}
WaitInMsec(150);
// armResetState = START_PID;
// break;
// case START_PID:
PresetIntegratedMotorEncoder(armR, 0);
PresetIntegratedMotorEncoder(armL, 0);
StartIntegratedMotorEncoderPID(armR, 0);
StartIntegratedMotorEncoderPID(armL, 0);
SetArmState(GROUND);
// armResetState = DONE;
// break;
// case DONE:
// break;
// }
}
int MotorCompliance(void)
{
int StopSwitch = 0;
int ForLoop = 1;
int ArmEmergencyRelease = 0;
unsigned int LetsGo = 0;
/* Bring the motor down at first powerup so the robot is compliant. */
while(ForLoop) // This is the same while loop as in the main program,
{ // but is using a variable set that starts set to 1
/* Get digital input from the switch mounted to the tower.
* GetDigitalInput is weird - 1 is unpressed, 0 is pressed */
StopSwitch = GetDigitalInput(7);
if(StopSwitch == 1)
{
/* Button on the joypad to press in case the above DI doesn't trigger.
* Same button as the arm unlock button. */
ArmEmergencyRelease = GetJoystickDigital(1,5,2);
if(ArmEmergencyRelease == 1)
{
SetServo(6,0);
ForLoop = 0;
}
/* Set various motors and servos to a compliant position */
SetMotor(2,-90);
SetServo(4,64);
SetServo(3,127);
SetServo(6,127);
SetServo(7,127);
}
else
{
LetsGo = GetJoystickAnalog(1,3);
if(LetsGo) //Move Raisy in any position where it won't be set to 0
{
SetServo(6,0); //Set Tilty to the straight position
ForLoop = 0; //Exit the while loop
}
else
{
SetMotor(2,-40); // Keep Raisy down to the base until the joystick is moved
}
}
}
/* Return 1 when this function is called. We need this set to 1 so this init function only runs
* once. MotorBringDown gets set to 1 and thus fails the if check when we do this. */
return 1;
}
void print_gd_debug_inf()
{
int i;
int ja[4] = {0, 0, 0, 0}; // joystick analog 1-4
unsigned char j5[2] = {0, 0}; // u, d
unsigned char j6[2] = {0, 0}; // u, d
unsigned char j7[4] = {0, 0, 0, 0}; // u, d, l, r
unsigned char j8[4] = {0, 0, 0, 0}; // u, d, l, r
int j_accelerometer[2] = {0, 0}; // x, y
for(i = 0; i < 4; i++){ja[i] = GetJoystickAnalog( 1 , i+1 ) ;}
j5[0] = GetJoystickDigital ( 1 , 5 , 2 ) ;
j5[1] = GetJoystickDigital ( 1 , 5 , 1 ) ;
j6[0] = GetJoystickDigital ( 1 , 6 , 2 ) ;
j6[1] = GetJoystickDigital ( 1 , 6 , 1 ) ;
j7[0] = GetJoystickDigital ( 1 , 7 , 2 ) ;
j7[1] = GetJoystickDigital ( 1 , 7 , 1 ) ;
j7[2] = GetJoystickDigital ( 1 , 7 , 3 ) ;
j7[3] = GetJoystickDigital ( 1 , 7 , 4 ) ;
j8[0] = GetJoystickDigital ( 1 , 8 , 2 ) ;
j8[1] = GetJoystickDigital ( 1 , 8 , 1 ) ;
j8[2] = GetJoystickDigital ( 1 , 8 , 3 ) ;
j8[3] = GetJoystickDigital ( 1 , 8 , 4 ) ;
j_accelerometer[0] = GetJoystickAccelerometer( 1 , 1 ) ;
j_accelerometer[1] = GetJoystickAccelerometer( 1 , 2 ) ;
PrintTextToGD ( 2 , 3 , 8388608 , "Channel 1 = %4d\n" , ja[0] ) ;
PrintTextToGD ( 3 , 3 , 8388608 , "Channel 2 = %4d\n" , ja[1] ) ;
PrintTextToGD ( 4 , 3 , 8388608 , "Channel 3 = %4d\n" , ja[2] ) ;
PrintTextToGD ( 5 , 3 , 8388608 , "Channel 4 = %4d\n" , ja[3] ) ;
PrintTextToGD ( 9 , 3 , 16384 , "Channel 5: Up = %d; Down = %d\n" , j5[0], j5[1] ) ;
PrintTextToGD ( 10 , 3 , 16384 , "Channel 6: Up = %d; Down = %d\n" , j6[0], j6[1] ) ;
PrintTextToGD ( 15 , 3 , 16744448 , "Channel 7: Up = %d; Down = %d; Left = %d; Right = %d\n" , j7[0], j7[1], j7[2], j7[3] ) ;
PrintTextToGD ( 16 , 3 , 16744448 , "Channel 8: Up = %d; Down = %d; Left = %d; Right = %d\n" , j8[0], j8[1], j8[2], j8[3] ) ;
PrintTextToGD ( 20 , 3 , 255 , "Gamepad Accelerometer: AxisX = %4d; AxisY = %4d\n" , j_accelerometer[0], j_accelerometer[1] ) ;
}
void main(void)
{
int Servo1 = 0;
int ServoFlagBits = 0;
int Servo2Test = 0;
int Openy1 = 0;
int Openy2 = 0;
int Openy3 = 0;
int Tilty1 = 0;
int Tilty2 = 0;
int Tilty3 = 0;
int Tilty4 = 0;
int Flippy1 = 0;
int Flippy2 = 0;
int Flippy3 = 0;
int Flippy4 = 0;
int MotorStop = 0;
int ProgramGo = 0;
int MotorBringDown = 0;
unsigned int ServoCount = 0;
unsigned int MotorPos = 0;
while(1)
{
/* Call the function that brings the arm down for compliance. */
if(MotorBringDown == 0) /* Run if variable is 0 (false) */
{
/* This call runs the function and puts the return value (1) into a variable.
* Since the above if statement checks if MotorBringDown is equal to 0 and we
* set it to 1, this call never runs again. */
MotorBringDown = MotorCompliance();
}
/* Motor Lock buttons
*
* Any GetJoystickDigital calls return 1 if pressed or 0 if not pressed. */
ProgramGo = GetJoystickDigital(1,5,1); //Channel 5, Button 1 (DOWN)
if(ProgramGo == 1)
{
MotorStop = 1; //Block all Raisy activity until this is set to 0
}
if(MotorStop == 1)
{
/* Run the motor at a small negative value so it stays in place.
* this works because of how the arm was balanced in 2013 */
SetMotor(2,-30);
/* Checking here to see if we need to unlock Raisy */
ProgramGo = GetJoystickDigital(1,5,2);
if(ProgramGo == 1) //If so...
{
MotorStop = 0; //...unlock it
}
}
else
{
/* Get the value of Joystick channel 3 and return it to an unsigned
* int since the joysticks range from -127 to 127 */
MotorPos = GetJoystickAnalog(1,3);
if(MotorPos > 10)
{
if(MotorPos < 50)
{
SetMotor(2,30); //Slow down the motor if MotorPos is greater than 50
}
else
{
SetMotor(2,MotorPos); //Set the motor speed to whatever is in MotorPos
}
}
else
{
SetMotor(2,-30); //Freeze Raisy if MotorPos is less than 10
}
}
/* Extendy. Plain and simple. */
JoystickToMotor(1,2,5,0);
/* Code for Openy */
Openy1 = GetJoystickDigital(1,6,1);
Openy3 = GetJoystickDigital(1,6,2);
if(Openy1 == 1)
{
SetServo(7,127);
Openy2 = 1;
}
else
{
if(Openy2 == 1 && Openy3 == 1)
{
SetServo(7,-127);
Openy2 = 0;
}
}
/* Code for Tilty */
Tilty1 = GetJoystickDigital(1,7,2);
Tilty2 = GetJoystickDigital(1,7,3);
Tilty3 = GetJoystickDigital(1,7,4);
if(Tilty1 == 1)
{
SetServo(6,0);
Tilty4 = 0;
}
else if(Tilty2 == 1)
{
SetServo(6,127);
Tilty4 = 1;
}
else if(Tilty3 == 1)
{
SetServo(6,-127);
Tilty4 = 1;
}
else
{
}
/* Code for Flippy */
Flippy1 = GetJoystickDigital(1,8,2);
Flippy2 = GetJoystickDigital(1,8,3);
Flippy3 = GetJoystickDigital(1,8,4);
if(Flippy1 == 1)
{
SetServo(3,-127);
SetServo(4,-64);
Flippy4 = 1;
}
else if(Flippy2 == 1)
{
SetServo(3,0);
SetServo(4,0);
Flippy4 = 1;
}
else if(Flippy3 == 1)
{
SetServo(3,127);
SetServo(4,64);
Flippy4 = 1;
}
/* Unfinished test code. Do not use. */
//Servo1 = GetJoystickDigital(1,8,2);
if(ServoFlagBits == 0)
{
if(Servo1 == 1)
{
for(ServoCount = 0; ServoCount < 128; ServoCount++)
{
//SetServo(9,ServoCount);
//Wait(5);
}
//ServoFlagBits = 1;
}
}
//Servo2Test = GetJoystickDigital(1,8,1);
if(ServoFlagBits == 1)
{
if(Servo2Test == 1)
{
/*
* for(ServoCount = 128; ServoCount --> 0;)
* {
* SetServo(9,ServoCount);
* }
* ServoFlagBits = 0;
*/
}
}
}
//Servo1 = 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) ;
}
}
}
/**
* get pressed state of the button
* @param joystick the joystick number
* @param button the constant representing the button
* @return 1=Pressed, 0=Idle
**/
int getJSDigital(int joystick, LV_BUTTON *button) {
return GetJoystickDigital(joystick, button->channel, button->number);
}
