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