Hi guys
I have created a PID controller, but it doesn’t work the way it should. I know it calculates the velocity correctly because i see the value in the debugger. The flywheel motors keep switching between 0 and 127. Someone please help me. I have posted my code below.
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Motor, port1, rightshooter1, tmotorVex393HighSpeed_HBridge, openLoop)
#pragma config(Motor, port2, rightshooter2, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port3, rightback, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port4, rightfront, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, intake1, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, intake2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port7, leftfront, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, leftback, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, leftshooter2, tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor, port10, leftshooter1, tmotorVex393HighSpeed_HBridge, openLoop, reversed)
//*!!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!
//global variables
float rpsL;
float rpsR;
float virtualspeed;
float shootingspeed;
float rotationsL;
float rotationsR;
float timeval;
//gloabal variables for PID
int shooterspeedL; //power sent to left motors
int shooterspeedR; //power sent to right motors
float shooterfactor;
float target; //stores desired speed
float ticksL; //ticks of left Motor Encoder
float ticksR; //ticks of right Motor Encoder
float time; //stores the elapsed time value
float velocityL; //velocity for left side
float velocityR; //velocity for right side
float errorL; //error for left side
float errorR; //error for right side
float errorTL; //total error for left side
float errorTR; //total error for right side
float proportionL; //value of proportional term for left side
float proportionR;//value of proportional term for right side
float integralL; //value of integral term for left side
float integralR;//value of integral term for right side
float derivitiveL; //value of derivative term for left side
float derivitiveR;//value of derivitive term for right side
float integralActiveZone = 400; //value in which the integral term is active
float lastErrorL; //stores previous error for left side
float lastErrorR; //stores previous error for right side
float Kp = .001;
float Ki = .000000001;
float Kd = 0;
task velocity_control() {
while(true) {
nMotorEncoder[leftshooter2] = 0;//clear the encoders
nMotorEncoder[rightshooter2] = 0;//clear the encoders
clearTimer(T1);
time = time1[T1] / 1000
ticksL = nMotorEncoder[leftshooter2];
ticksR = nMotorEncoder[rightshooter2];
rotationsL = ticksL / 392;
rotationsR = ticksR / 392;
velocityL = (rotationsL / time) * ((49 / 3);
velocityR = (rotationsR / time) * ((49 / 3);
errorL = target - velocityL;
errorR = target - velocityR;
if(errorL<integralActiveZone && errorL != 0) {
errorTL += errorL;
}
else {
errorTL = 0;
}
if(errorR<integralActiveZone && errorR != 0) {
errorTR += errorR;
}
else {
errorTR = 0;
}
if(errorTL > 50/Ki) {
errorTL = 50/Ki;
}
if(errorTR > 50/Ki) {
errorTR = 50/Ki;
}
if(errorL == 0) {
derivitiveL = 0;
}
if(errorR == 0) {
derivitiveR = 0;
}
proportionL = errorL * Kp;
proportionR = errorR * Kp;
integralL = errorTL * Ki;
integralR = errorTR * Ki;
derivitiveL = (errorL - lastErrorL) * Kd;
derivitiveR = (errorR - lastErrorR) * Kd;
lastErrorL = errorL;
lastErrorR = errorR;
shooterspeedL = proportionL + integralL + derivitiveL;
shooterspeedR = proportionR + integralR + derivitiveR;
motor[leftshooter1] = motor[leftshooter2] = shooterspeedL;
motor[rightshooter1] = motor[rightshooter2] = shooterspeedR;
}
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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;
// 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()
{
//clearTimer(T1);
//launch();
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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() {
while (true)
{
startTask(velocity_control);
slaveMotor(intake1, intake2);
while(vexRT[Btn5U] == 1) {
motor[leftfront] = motor[leftback] = vexRT[Ch2];
motor[rightfront] = motor[rightback] = vexRT[Ch3];
}
motor[leftfront] = motor[leftback] = vexRT[Ch3];
motor[rightfront] = motor[rightback] = vexRT[Ch2];
if(vexRT[Btn5UXmtr2] == 1) {
motor[intake1] = -80;
motor[intake2] = -80;
}
else if(vexRT[Btn5DXmtr2] == 1) {
motor[intake1] = 80;
motor[intake2] = 80;
}
else {
motor[intake1] = 0;
motor[intake2] = 0;
}
slaveMotor(rightshooter1, rightshooter2);
slaveMotor(leftshooter1, leftshooter2);
if(vexRT[Btn8RXmtr2] ==1) {
target = 500;
}
else if(vexRT[Btn8UXmtr2] ==1) {
}
else if(vexRT[Btn8LXmtr2] ==1) {
}
else if(vexRT[Btn8DXmtr2] == 1) {
}
else {
target = 0;
}
}
}