I am using ROBOTC built in PID to slave two motors for my flywheel. When I tried to give power to the intake after I start the flywheel it does not work in autonomous mode. I am including the code.
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1, limitswitch, sensorTouch)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_4, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port2, rearRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, frontRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, rearLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, BLfly, tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor, port6, Intake, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, BRfly, tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_4)
#pragma config(Motor, port8, TRfly, tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor, port9, TLfly, tmotorVex393TurboSpeed_MC29, openLoop, reversed, encoderPort, I2C_4)
#pragma config(Motor, port10, frontLeft, tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(115)
#include "Vex_Competition_Includes_withPID.c" //Main competition background code...do not modify!
/////////////////////////////////////////////////////////////////////////////////////////
//
// Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
bLCDBacklight = true;
clearLCDLine(0);
clearLCDLine(1);
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous()
{
while (bIfiAutonomousMode)
{
// new code for testing;
//original intake value is 50 and flywheel speed was 63
clearLCDLine(1);
displayLCDPos(1, 0);
motor[TLfly] = 49;
motor[TRfly] = 49;
wait1Msec(2000);
motor[Intake] = 80;
int rpm = getMotorVelocity(TLfly);
displayLCDNumber(1, 0, rpm);
if (rpm <= 146) {
motor[Intake] = 0;
}
else {
motor[Intake] = 80;
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task usercontrol()
{
// User control code here, inside the loop
while (true)
{
clearLCDLine(1); // Clear line 2 (1) of the LCD
motor[frontLeft] = vexRT[Ch3];// Left Joystick Y value
motor[rearLeft] = vexRT[Ch3];
motor[frontRight] = vexRT[Ch2]; // Right Joystick Y value
motor[rearRight] = vexRT[Ch2];
motor[Intake] = vexRT[Btn5U]; // This button is for the intake
if (vexRT[Btn6U] == 1)
{
motor[TLfly] = 49;
motor[TRfly] = 49;
}
if (vexRT[Btn6D] == 1)
{
motor[TLfly] = 40;
motor[TRfly] = 40;
}
clearLCDLine(1); // Clear line 2 (1) of the LCD
displayLCDPos(1, 0);
int rpm = getMotorVelocity(TLfly);
displayLCDNumber(1, 0, rpm);
if (vexRT[Btn5U] == 1)
{
motor[Intake] = 127;
}
if (vexRT[Btn5D] == 1)
{
motor[Intake] = -127;
}
if ((vexRT[Btn5D] == 0) && (vexRT[Btn5U] == 0) && (vexRT[Btn6D] == 0) && (vexRT[Btn6U] == 0))
{
motor[TRfly] = 0;
motor[TLfly] = 0;
motor[Intake] = 0;
motor[BLfly] = 0;
motor[BRfly] = 0;
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
//
// VEX Competition Control Include File
//
// This file provides control over a VEX Competition Match. It should be included in the user's
// program with the following line located near the start of the user's program
// #include "VEX_Competition_Includes.h"
// The above statement will cause this program to be included in the user's program. There's no
// need to modify this program.
//
// The program displays status information on the new VEX LCD about the competition state. You don't
// need the LCD, the program will work fine whether or not the LCD is actually provisioned.
//
// The status information is still useful without the LCD. The ROBOTC IDE debugger has a "remote screen"
// that contains a copy of the status information on the LCD. YOu can use this to get a view of the
// status of your program. The remote screen is shown with the menu command
// "Robot -> Debugger Windows -> VEX Remote Screen"
//
// The LCD is 2 lines x 16 characters. There are three display formats to look for:
//
// State Description
//
// ----------------
// |Disabled | The robot is idle. This occurs before both the autonomous and user
// |Time mm:ss.s | control states. The time display is minutes and seconds it has been idle.
// ----------------
//
// ----------------
// |Autonomous | The robot is running autonomous code.
// |Time mm:ss.s | control states. The time display is minutes and seconds it has been running.
// ----------------
//
// ----------------
// |User Control | The robot is running user control code.
// |Time mm:ss.s | control states. The time display is minutes and seconds it has been running.
// ----------------
//////////////////////////////////////////////////////////////////////////////////////////////////////
void allMotorsOff();
void allTasksStop();
void pre_auton();
task autonomous();
task usercontrol();
int nTimeXX = 0;
bool bStopTasksBetweenModes = true;
static void displayStatusAndTime();
task main()
{
// Master CPU will not let competition start until powered on for at least 2-seconds
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString("Startup");
wait1Msec(2000);
resetMotorEncoder(BLfly); // reset encoders
resetMotorEncoder(BRfly);
resetMotorEncoder(TLfly);
resetMotorEncoder(TRfly);
slaveMotor(BLfly, TLfly);
slaveMotor(BRfly, TRfly);
pre_auton();
//wait1Msec(500);