コード例 #1
0
ファイル: autonDrive.c プロジェクト: AntonOlsen/vrc-9999
void autonDrive( int speed, int inches ) {

	float inchesL=0;
	float inchesR=0;
	int timeout=0;

	resetImeLeft();
	resetImeRight();

    // Start to motors
    vexMotorSet( MotorDriveL1, speed-2 );
    vexMotorSet( MotorDriveL2, speed-2 );
    vexMotorSet( MotorDriveR1, speed );
    vexMotorSet( MotorDriveR2, speed );

	while( TRUE ) {
		vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_2, "L %5.2f R %5.2f", getInchesLeft(), getInchesRight() );
		vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_2, "L %5.2f R %5.2f", getInchesLeft(), getInchesRight() );

		if (inchesL == getInchesLeft() && inchesR == getInchesRight() ) {
			timeout++;
			if( timeout > 30 ) {
				break;
			}
		}
		inchesL = getInchesLeft();
		inchesR = getInchesRight();

		if( abs(inchesL) > inches || abs(inchesR) > inches ) {
			break;
		}
		vexSleep(5);
	}

	// Reverse motors to stop coast
    vexMotorSet( MotorDriveL1, -speed/2 );
    vexMotorSet( MotorDriveL2, -speed/2 );
    vexMotorSet( MotorDriveR1, -speed/2 );
    vexMotorSet( MotorDriveR2, -speed/2 );

    vexSleep( abs(speed/8) );

    // Stop motors
    vexMotorSet( MotorDriveL1, 0 );
    vexMotorSet( MotorDriveL2, 0 );
    vexMotorSet( MotorDriveR1, 0 );
    vexMotorSet( MotorDriveR2, 0 );

}
コード例 #2
0
ファイル: vexuser.c プロジェクト: Impact2585/convex
// Driver control task
msg_t
vexOperator( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("operator");

    // Start manual driving
    StartTask( DriveTask );

    // start the Arm PID task
    StartTask( ArmPidController );
    // start the claw motor controller
    StartTask( ClawController );

    // start manual arm/claw control
    StartTask( ManualArmClawTask );

    // Run until asked to terminate
    while(!chThdShouldTerminate())
        {
        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}
コード例 #3
0
ファイル: vexuser.c プロジェクト: Impact2585/convex
/** @details
 *  This thread is started when the driver control period is started
 */
msg_t
vexOperator( void *arg )
{
	int16_t		blink = 0;

	(void)arg;

	// Must call this
	vexTaskRegister("operator");

	// Run until asked to terminate
	while(!chThdShouldTerminate())
		{
		// flash led/digi out
		vexDigitalPinSet( kVexDigital_1, (blink++ >> 3) & 1);

		// status on LCD of encoder and sonar
		vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_1, "%4.2fV   %8.1f", vexSpiGetMainBattery() / 1000.0, chTimeNow() / 1000.0 );
		vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_2, "L %3d R %3d", vexMotorGet( MotorDriveL ), vexMotorGet( MotorDriveR ) );

		// Tank drive
		// left drive
		vexMotorSet( MotorDriveL, vexControllerGet( Ch3 ) );

		// right drive
		vexMotorSet( MotorDriveR, vexControllerGet( Ch2 ) );

		// Don't hog cpu
		vexSleep( 25 );
		}

	return (msg_t)0;
}
コード例 #4
0
ファイル: vexuser.c プロジェクト: Impact2585/convex
// Autonomous control task
msg_t
vexAutonomous( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("auton");

    // start the Arm PID task
    StartTask( ArmPidController );
    // start the claw motor controller
    StartTask( ClawController );

    // Arm Up
    SetArmPosition( 2000 );
    // Claw Open
    ClawOpen();
    wait1Msec(2000);

    // Move forward
    DriveSystemArcadeDrive( 100, 0 );
    wait1Msec(1000);
    // Stop drivw
    DriveSystemArcadeDrive( 0, 0 );

    // Arm down
    SetArmPosition( 700 );
    wait1Msec(2000);

    // close claw
    ClawClose();
    wait1Msec(1000);

    // Arm up
    SetArmPosition( 1000 );
    wait1Msec(1000);
    // Turn
    DriveSystemArcadeDrive( 0, 100 );
    wait1Msec(1000);
    // Stop drivw
    DriveSystemArcadeDrive( 0, 0 );

    // Claw Open
    ClawOpen();
    wait1Msec(1000);

    // Stop claw motor
    ClawStop();

    while(1)
        {
        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}
コード例 #5
0
ファイル: vexuser.c プロジェクト: Impact2585/convex
void
RunPattern( short *p, short len, short delayMs )
{
    int i = 0;

    for(i=0;i<len;i++)
        {
        display( *p++ );
        vexSleep(delayMs);
        }
}
コード例 #6
0
/** @details
 *  This thread is started when the driver control period is started
 */
msg_t
vexOperator( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("operator");

    vexLcdClearLine(VEX_LCD_DISPLAY_1, VEX_LCD_LINE_T);
    vexLcdClearLine(VEX_LCD_DISPLAY_1, VEX_LCD_LINE_B);

    // Run until asked to terminate
    while(!chThdShouldTerminate())
        {
        // show some user parameters
        vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_T, "%02X %02X %02X %02X", userp->data[0],userp->data[1],userp->data[2],userp->data[3]);

        // buttons 8 change user parameters
        if( vexControllerGet( Btn8U ) == 1 )
            {
            userp->data[0]++;
            vexControllerReleaseWait(Btn8U);
            }
        if( vexControllerGet( Btn8R ) == 1 )
            {
            userp->data[1]++;
            vexControllerReleaseWait(Btn8R);
            }
        if( vexControllerGet( Btn8D) == 1 )
            {
            userp->data[2]++;
            vexControllerReleaseWait(Btn8D);
            }
        if( vexControllerGet( Btn8L) == 1 )
            {
            userp->data[3]++;
            vexControllerReleaseWait(Btn8L);
            }

        // Button 7 Up saves
        if( vexControllerGet( Btn7U ) == 1 )
            {
            vexFlashUserParamWrite( userp );
            vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_B, "Saved...");
            vexControllerReleaseWait(Btn7U);
            }

        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}
コード例 #7
0
ファイル: pidbot.c プロジェクト: Impact2585/vexell
task c_vexAutonomous(void *arg) {
  (void)arg;
  vexTaskRegister("auton");

  StartTask(armTask);
  autonomous();

  while (!chThdShouldTerminate()) {
    vexSleep(25);
  }

  return (task)0;
}
コード例 #8
0
ファイル: pidbot.c プロジェクト: Impact2585/vexell
task c_vexOperator(void *arg) {
  (void)arg;
  vexTaskRegister("operator");

  StartTask(driveTask); // drive
  StartTask(armTask); // arm
  StartTask(intakeTask); // intake
  StartTask(pneumaticsTask); // pneumatics

  while (!chThdShouldTerminate()) { // sleep forever
    vexSleep(25);
  }

  return (task)0;
}
コード例 #9
0
ファイル: vexuser.c プロジェクト: Impact2585/convex
// Autonomous control task
msg_t
vexAutonomous( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("auton");

    while(1)
        {
        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}
コード例 #10
0
ファイル: misc.c プロジェクト: Impact2585/pidbot-convex
task safetyTask(void *arg) {
  (void)arg;
  vexTaskRegister("safety");

  while (!chThdShouldTerminate()) {
    // detect if controller is face down.
    if (vexControllerGet(AcclY) < 0) { // figure this out
      vexMotorStopAll(); // kill all motors (for now)
      //vexTaskEmergencyStop(); // emergency stop, reboot required
    }

    vexSleep(10);
  }

  return (task)0;
}
コード例 #11
0
ファイル: vexuser.c プロジェクト: Impact2585/convex
// Driver control task
msg_t
vexOperator( void *arg )
{
	(void)arg;
	int     delay, i;

	// Must call this
	vexTaskRegister("operator");

    while(1)
        {
        // run a pattern with speed 50
        for(delay=100;delay>10;delay-=10)
            RunPattern( &pattern_0[0], sizeof(pattern_0)/sizeof(short), delay );

        for(i=0;i<20;i++)
            RunPattern( &pattern_0[0], sizeof(pattern_0)/sizeof(short), 10 );

        // not really needed but will leave here
        vexSleep(10);
        }

	return (msg_t)0;
}
コード例 #12
0
// Driver control task
msg_t
vexOperator( void *arg )
{
	int16_t		forward;
	int16_t		turn;
	int16_t     nexttune = 0;

	(void)arg;

	// Must call this
	vexTaskRegister("operator");

    RtttlDemo(nexttune);

	// Run until asked to terminate
	while(!chThdShouldTerminate())
		{
	    if( vexControllerGet( Btn7 ) )
	        {
	        if( vexControllerGet( Btn7R ) )
	            {
	            if(++nexttune == NUM_TUNES)
	                nexttune = 0;
	            }
	        if( vexControllerGet( Btn7L ) )
	            {
	            if(--nexttune < 0)
	                nexttune = NUM_TUNES-1;
	            }

	        RtttlDemo(nexttune);

	        // lazy
	        while(vexControllerGet( Btn7 ))
	            vexSleep(25);
	        }

		// status on LCD of encoder and sonar
		vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_1, "Batt %4.2fV", vexSpiGetMainBattery() / 1000.0 );

		// Get controller
		if( abs( vexControllerGet( Ch2 )) > 10 )
			forward = vexControllerGet( Ch2 );
		else
			forward = 0;

		if( abs( vexControllerGet( Ch1 )) > 10 )
			turn = vexControllerGet( Ch1 );
		else
			turn = 0;

        if( abs( vexControllerGet( Ch3 )) > 10 )
            SetMotor( 1, vexControllerGet( Ch3 ) );
        else
            SetMotor( 1, 0 );

		// arcade drive
		DriveSystemArcadeDrive( forward, turn );

		// Don't hog cpu
		vexSleep( 25 );
		}

	return (msg_t)0;
}
コード例 #13
0
/** @details
 *  This function simulates the cortex default code, it was based on the ROBOTC
 *  implementation but with some changes in structure.
 */
void
vexCortexDefaultDriver(void)
{
    int16_t mech1_lim_ccw = 0;
    int16_t mech1_lim_cw  = 0;
    int16_t mech2_lim_ccw = 0;
    int16_t mech2_lim_cw  = 0;
    int16_t mech3_lim_ccw = 0;
    int16_t mech3_lim_cw  = 0;
    int16_t mech4_lim_ccw = 0;
    int16_t mech4_lim_cw  = 0;

    int16_t drive_left  = 0;
    int16_t drive_right = 0;
    int16_t drive_mech1 = 0;
    int16_t drive_mech2 = 0;
    int16_t drive_mech3 = 0;
    int16_t drive_mech4 = 0;

    while( !chThdShouldTerminate() )
        {
        // Set motor direction - a jumper in ports 1 through 10 reversed motor
        vexMotorDirectionSet( LeftDrive1,   vexDigitalPinGet( kVexDigital_1  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( LeftDrive2,   vexDigitalPinGet( kVexDigital_2  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( LeftDrive3,   vexDigitalPinGet( kVexDigital_3  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( RightDrive4,  vexDigitalPinGet( kVexDigital_4  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( RightDrive5,  vexDigitalPinGet( kVexDigital_5  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( Mech1,        vexDigitalPinGet( kVexDigital_6  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( Mech2,        vexDigitalPinGet( kVexDigital_7  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( Mech3,        vexDigitalPinGet( kVexDigital_8  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( Mech4,        vexDigitalPinGet( kVexDigital_9  ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
        vexMotorDirectionSet( RightDrive10, vexDigitalPinGet( kVexDigital_10 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );

        // Sample analog inputs - these are used as limit switches
        mech1_lim_ccw = vexAdcGet( kVexAnalog_1 ) < 200 ? 0 : 1;
        mech1_lim_cw  = vexAdcGet( kVexAnalog_2 ) < 200 ? 0 : 1;
        mech2_lim_ccw = vexAdcGet( kVexAnalog_3 ) < 200 ? 0 : 1;
        mech2_lim_cw  = vexAdcGet( kVexAnalog_4 ) < 200 ? 0 : 1;
        mech3_lim_ccw = vexAdcGet( kVexAnalog_5 ) < 200 ? 0 : 1;
        mech3_lim_cw  = vexAdcGet( kVexAnalog_6 ) < 200 ? 0 : 1;
        mech4_lim_ccw = vexAdcGet( kVexAnalog_7 ) < 200 ? 0 : 1;
        mech4_lim_cw  = vexAdcGet( kVexAnalog_8 ) < 200 ? 0 : 1;

        // SINGLE DRIVER - TANK
        if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_OUT && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_OUT )
            {
            drive_left  =  vexControllerGet( Ch3 ); // up = CW
            drive_right = -vexControllerGet( Ch2 ); // up = CCW

            drive_mech1 = (vexControllerGet( Btn5U ) * 127) - (vexControllerGet( Btn5D ) * 127); // U = CW, D = CCW
            drive_mech2 = (vexControllerGet( Btn6U ) * 127) - (vexControllerGet( Btn6D ) * 127); // U = CW, D = CCW
            drive_mech3 = (vexControllerGet( Btn7U ) * 127) - (vexControllerGet( Btn7D ) * 127); // U = CW, D = CCW
            drive_mech4 = (vexControllerGet( Btn8U ) * 127) - (vexControllerGet( Btn8D ) * 127); // U = CW, D = CCW
            }
        else
        // DUAL DRIVER - TANK
        if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_IN && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_OUT )
            {
            drive_left  =  vexControllerGet( Ch3 ); // up = CW
            drive_right = -vexControllerGet( Ch2 ); // up = CCW

            drive_mech1 = (vexControllerGet( Btn5UXmtr2 ) * 127) - (vexControllerGet( Btn5DXmtr2 ) * 127); // U = CW, D = CCW
            drive_mech2 = (vexControllerGet( Btn6UXmtr2 ) * 127) - (vexControllerGet( Btn6DXmtr2 ) * 127); // U = CW, D = CCW
            drive_mech3 =  vexControllerGet( Ch3Xmtr2 ); // up = CW
            drive_mech4 =  vexControllerGet( Ch2Xmtr2 ); // up = CW
            }
        else
        // SINGLE DRIVER - ARCADE
        if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_OUT && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_IN )
            {
            drive_left  =  vexControllerGet( Ch1 ) + vexControllerGet( Ch2 ); // up = CW,  right = CW
            drive_right =  vexControllerGet( Ch1 ) - vexControllerGet( Ch2 ); // up = CCW, right = CW

            drive_mech1 = (vexControllerGet( Btn5U ) * 127) - (vexControllerGet( Btn5D ) * 127); // U = CW, D = CCW
            drive_mech2 = (vexControllerGet( Btn6U ) * 127) - (vexControllerGet( Btn6D ) * 127); // U = CW, D = CCW
            drive_mech3 =  vexControllerGet( Ch3 ); // up = CW
            drive_mech4 = -vexControllerGet( Ch4 ); // right = CCW
            }
        else
        // DUAL DRIVER - ARCADE
        if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_IN && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_IN )
            {
            drive_left  =  vexControllerGet( Ch1 ) + vexControllerGet( Ch2 ); // up = CW,  right = CW
            drive_right =  vexControllerGet( Ch1 ) - vexControllerGet( Ch2 ); // up = CCW, right = CW

            drive_mech1 = (vexControllerGet( Btn5UXmtr2 ) * 127) - (vexControllerGet( Btn5DXmtr2 ) * 127); // U = CW, D = CCW
            drive_mech2 = (vexControllerGet( Btn6UXmtr2 ) * 127) - (vexControllerGet( Btn6DXmtr2 ) * 127); // U = CW, D = CCW
            drive_mech3 =  vexControllerGet( Ch3Xmtr2 ); // up = CW
            drive_mech4 =  vexControllerGet( Ch2Xmtr2 ); // up = CW
            }

        // Left drive motors
        vexMotorSet( LeftDrive1,   drive_left  );
        vexMotorSet( LeftDrive2,   drive_left  );
        vexMotorSet( LeftDrive3,   drive_left  );

        // Right drive motors
        vexMotorSet( RightDrive4,  drive_right );
        vexMotorSet( RightDrive5,  drive_right );
        vexMotorSet( RightDrive10, drive_right );

        // Limit switches
        if( ( !mech1_lim_cw && (drive_mech1 > 0)) || ( !mech1_lim_ccw && (drive_mech1 < 0)) )
            drive_mech1 = 0;

        if( ( !mech2_lim_cw && (drive_mech2 > 0)) || ( !mech2_lim_ccw && (drive_mech2 < 0)) )
            drive_mech2 = 0;

        if( ( !mech3_lim_cw && (drive_mech3 > 0)) || ( !mech3_lim_ccw && (drive_mech3 < 0)) )
            drive_mech3 = 0;

        if( ( !mech4_lim_cw && (drive_mech4 > 0)) || ( !mech4_lim_ccw && (drive_mech4 < 0)) )
            drive_mech4 = 0;

        // Mechanism motors
        vexMotorSet( Mech1, drive_mech1 );
        vexMotorSet( Mech2, drive_mech2 );
        vexMotorSet( Mech3, drive_mech3 );
        vexMotorSet( Mech4, drive_mech4 );

        // small delay
        vexSleep(10);
        }
}