I have a four motor ROBOTC PID enabled flywheel. I have configured two motors with IME as master and the second motor on each side is configured as slave motor. I am trying to write a program on how to control intake while flywheel is getting to the target velocity especially in the autonomus mode. If I start the motor after the flywheel motors are enabled intake does not even start. I am trying to figure out why this is not working. Does the PID control spun a different task and does not bring the control back to execute the commands after it.
I would appreciate if anybody can look over the code and give their advice.
#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(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_2)
#pragma config(Motor, port8, TRfly, tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port9, TLfly, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#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)
//task feederControl();
//void feederWait();
//#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
#include "flywheel_s.c" // included on 02/10/2016 for setting target velocity.
// #include "flywheel.c" // included on 02/04/2016
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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;
nMotorEncoder[BLfly]=0;
nMotorEncoder[BRfly]=0;
nMotorEncoder[TRfly]=0;
nMotorEncoder[TLfly]=0;
// 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 (true)
{
displayLCDPos(1, 0);
displayNextLCDString("in the autonomous");
// Motors on ports 3, 4 & 5 will follow the motor on port 2
slaveMotor(BLfly, TLfly);
slaveMotor(BRfly, TRfly);
motor[TLfly] = 53;
motor[TRfly] = 53;
wait1Msec(1550);
motor[Intake] = 55;
// int rpm = getMotorVelocity(BLfly);
float rpm = calculateVelocityBetter(BLfly, 15);
string drpm;
displayLCDNumber(1, 0, rpm);
wait1Msec(100);
}
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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
// srini commented out 01/23/2016
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] = 55;
motor[TRfly] = 55;
motor[BLfly] = 55;
motor[BRfly] = 55;
}
if (vexRT[Btn6D] == 1)
{
motor[TLfly] = 52;
motor[TRfly] = 52;
motor[BLfly] = 52;
motor[BRfly] = 52;
}
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;
}
}
}
task main()
{
// Master CPU will not let competition start until powered on for at least 2-seconds
bLCDBacklight=true;
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString("Startup");
wait1Msec(2000);
pre_auton();
//wait1Msec(500);
while (true)
{
startTask(autonomous);
//startTask(feederControl);
// feederWait();
}
}