Hi, I am newbie to Robotc. I try to use the internal PID. But I now can’t change motor speed after PID is enabled using Motor and Sensor Configuration – the motor always run on the same speed even if I change the value of motor] in launchBallLeft task. May someone help me?
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1, mode1, sensorDigitalIn)
#pragma config(Sensor, dgtl2, mode2, sensorDigitalIn)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port2, baseR, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port3, baseR, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port4, lift1, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port5, launch1, tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor, port6, launch2, tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor, port7, grab, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, bassL, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, baseL, tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.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.
//
/////////////////////////////////////////////////////////////////////////////////////////
int firstSpeed;
int secondSpeed;
int thirdSpeed;
int V1;
void setSpeed(int a, int b, int c){
firstSpeed = a;
secondSpeed = b;
thirdSpeed = c;
}
task baseControl(){
while(true){
motor[port2] = vexRT[Ch3]-vexRT[Ch1]*1.5; //right
motor[port3] = vexRT[Ch3]-vexRT[Ch1]*1.5;
motor[port8] = vexRT[Ch3]+vexRT[Ch1]*1.5;
motor[port9] = vexRT[Ch3]+vexRT[Ch1]*1.5;
}
}
task shiftBase(){
while(true){
if(vexRT[Btn8R] == 1){
clearTimer(T1);
while(time1[T1] < 150){
motor[port2] = -75;
motor[port3] = -75;
motor[port8] = 75;
motor[port9] = 75;
}
}
if(vexRT[Btn8L] == 1){
clearTimer(T1);
while(time1[T1] < 150){
motor[port2] = 75;
motor[port3] = 75;
motor[port8] = -75;
motor[port9] = -75;
}
}
else if(time1[T1] >150){
motor[port2] = 0;
motor[port3] = 0;
motor[port8] = 0;
motor[port9] = 0;
}
}
}
task lift(){
while(true){
if(vexRT[Btn6U] == 1){
motor[port4] = 127;
}
if(vexRT[Btn6D] == 1){
motor[port4] = -127;
}
else if(vexRT[Btn6D] == 0 && vexRT[Btn6U] == 0){
motor[port4] = 0;
}
}
}
task launchBallLeft(){
int speed = 0;
resetSensor(I2C_1);
while(true){
if(vexRT[Btn7U] == 1){
speed = firstSpeed;
motor[port5] = speed;
motor[port6] = speed;
}
if(vexRT[Btn7L] == 1){
speed = secondSpeed;
motor[port5] = speed;
motor[port6] = speed;
}
if(vexRT[Btn7R] == 1){
speed = thirdSpeed;
motor[port5] = 50;
motor[port6] = 50;
}
else if(vexRT[Btn7D] == 1){
speed = 0;
motor[port5] = speed;
motor[port6] = speed;
}
}
}
task launchBallRight(){
int speed = 0;
setPIDforMotor(launch1,true);
setPIDforMotor(launch2,true);
while(true){
if(vexRT[Btn8U] == 1){
speed = firstSpeed;
motor[port5] = speed;
motor[port6] = speed;
}
if(vexRT[Btn8D] == 1){
speed = secondSpeed;
motor[port5] = speed;
motor[port6] = speed;
}
else if(vexRT[Btn8D] == 0 && vexRT[Btn8U] == 0){
speed = 0;
motor[port5] = speed;
motor[port6] = speed;
}
}
}
task grabControl(){
while(true){
if(vexRT[Btn5U] == 1){
motor[port7] = -127;
}
else if(vexRT[Btn5D] == 1){
motor[port7] = 127;
}
else if(vexRT[Btn5D] == 0 && vexRT[Btn5U] == 0){
motor[port7] = 0;
}
}
}
task autoMode1(){
clearTimer(timer1);
while(true){
motor[port5] = 80;
motor[port6] = 80;
motor[port7] = 0;
if(time1[T1] == 3000){
while(time1[T1]<=3500){
motor[port7] = -127;
}
clearTimer(timer1);
}
}
}
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;
// 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(){
if(SensorValue[mode1] == 0){
startTask(autoMode1);
}
if(SensorValue[mode2] == 0){
while(true){
motor[port2] = 80;
motor[port3] = 80;
motor[port8] = 80;
motor[port9] = 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
setSpeed(99,94,50);
while (true)
{
V1 = getMotorVelocity(launch1);
if(vexRT[Btn7U] == 1 || vexRT[Btn7L] == 1 || vexRT[Btn7R] == 1){
startTask(launchBallLeft);
stopTask(launchBallRight);
}
else if(vexRT[Btn8U] == 1 || vexRT[Btn8D] == 1){
stopTask(launchBallLeft);
startTask(launchBallRight);
}
startTask(baseControl);
startTask(grabControl);
startTask(lift);
startTask(shiftBase);
}
}