NBN Teams - Post your code !

VEX V5 General Discussion Technical Discussion
#1

The NBN competition season is over, congratulations to everyone who participated, I think you all probably improved your programming skills this year. So my challenge to you all now, especially the world champions, is “post your code”. Lets see all the clever PID, TBH and Bang Bang control loops. How did you keep your puncher synchronized with the balls, did you so something special with your drive. Lets start sharing this before the knowledge is lost so everyone can learn from it.

(if there is enough interest I will setup a dedicated repo for all the code, for anyone who shares without a license we will assume it is under a permissive open source license so anyone can use it).

1 Like
#2

I’m excited for this! I just made the repo for our Worlds bot public - you can find it here: https://github.com/team62/2016MarkIII
Please excuse my undescriptive commit messages and lack of comments throughout.

I still have to push competition changes but I’ll do that when I have my laptop available.
The last file i used was worlds[largest_number].c

For our double puncher, the code is: https://github.com/team62/2016MarkII

For our first single flywheel, the code is: https://github.com/team62/2016

I would be very interested in a central repo - perhaps there’s a way to do that really easily with submodules.

1 Like
#4

Beware, this is pretty long, but very well commented.
WorldsCode.pdf (403 KB)

1 Like
#5

Well, our flywheel code consists of Jpearmans TBH code Completely. But after our season was done I worked on a bang-bang code that I haven’t tried out yet because I don’t have our bot, I am curious to see if it works though. So if anyone has the time any constructive criticism would be appreciated. I would post our main code but there wasn’t anything special about it so, yeah. Here’s the bang-bang code

#pragma config(Motor,  port1,           motor1,        tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//////////////////////////////
//This is a test file to try//
//and test my bang-bang code//
//////////////////////////////
int speed;
int finalSpeed;
int target;//This is the target speed
float gain;//This is the value at which the flywheel will spin up

task gainValue()
{
	while(finalSpeed < target)//While the motors are less than the target value, run the motors at 127 power
	{
		finalSpeed = speed + gain;//increase the motor power by whatever gain is set to
		wait1Msec(10);
	}
}

task bangbang()
{

	if(finalSpeed < target)//If the motor is less than the set target
	{
		startTask(gainValue, 12);//start the gain value task
	}

	if(finalSpeed >= target)//once the motor has reached or passed the target
	{
		finalSpeed = target;//Make the motor equal the target
	}
}

task main()
{

	motor[motor1] = finalSpeed;//The flywheel motor equals finalspeed

	startTask(bangbang, 10);//Start the bang bang task

	while(1 == 1)//A while loop, set to loop forever
	{
		if(vexRT[Btn5D] == 1)//If button 5 d is pressed
		{
			target = 60;//The target is 60 motor power
			gain = 4.5;//Move the motor up by increments of 4.5 motor power
		}
	}
}

The code basically makes it so that if the motor is below the set target then the motor moves up in increments based on the gain value until the motor equals or goes over the target value. If anyone has the time it would be appreciated if you could give me some tips and pointers as this is my first time making any sort of control code from scratch.

1 Like
#6


Sorry for the lack of comments… I’m not very diligent in typing them (I’m the only programmer on my team).
Also, that PI part was never used, but it stayed in my program as back up (it was slapped together, so no tuning).
The automatic base speed adjustment starts 1 second after the flywheel is done with full speed.

1 Like
#7

Hi,

This is 2915A’s code, its pretty simple. Bang bang on the way up, and proportional on the way down. The only thing different to most people is that it calibrates before a game to find out the power to base its calculations around.

float fullCourtTargetSpeed = 39;
float fullCourtStartSpeed = 66;
float skillsTargetSpeed = 28;
float skillsStartSpeed = 49;
float shortTargetSpeed = 25.5;
float shortStartSpeed = 39;
float targetSpeed = 0;
float startSpeed = 0;
float flywheelKp = 16.5;
float lastHalfVelocity = 0;
float velocity = 0, power = 0;
bool flywheelRunning = false;
bool calibrated = false;

void calibrate()
{
	calibrated = false;
	time1[t2] = 0;
	bool going = true;
	setFlywheel(fullCourtStartSpeed);
	wait1Msec(5000);
	while(going)
	{
		//Wait for time, use timers instead of waits to account for processing time
		while(time1[T1] < timeDifference){}
		time1[T1] = 0;
		//Calculate average velocity over both encoders
		velocity = (SensorValue[enc] + SensorValue[encB]) /2;
		SensorValue[enc] = 0;
		SensorValue[encB] = 0;
		//Set flywheel to last calibrated value
		setFlywheel(fullCourtStartSpeed);
		//Wait for flywheel to get up to speed
		if(time1[T2] > 2000)
		{
			//If it is close enough to the right speed, exit
			if(abs(velocity - fullCourtTargetSpeed) < 2)
			{
				going = false;
				setFlywheel(0);
			}
			//If it is going too slow, increase start speed
			else if(velocity < fullCourtTargetSpeed)
				fullCourtStartSpeed++;
			//If it is going too fast, decrease start speed
			else if(velocity > fullCourtTargetSpeed)
				fullCourtStartSpeed--;
			time1[T2] = 0;
		}
	}
}

task flywheelControl()
{

	//Run feeder backwards to keep balls away from flywheel when starting up
	flywheelRunning = true;
	cLock = true;
	motor[conveyor] = -127;
	wait1Msec(20);
	motor[conveyor] = 0;
	Sensorvalue[enc] = 0;

	while(true){
		//Wait for time, use timers instead of waits to account for processing time
		while(time1[T1] < timeDifference/2){}
		time1[T1] = 0;

		//Calculate half time velocity, Velocity is encoder displacement in half of timeDifference
		float currentHalfVelocity =  (float)(SensorValue[enc] + SensorValue[encB]) /2
		velocity = currentHalfVelocity + lastHalfVelocity;
		lastHalfVelocity = currentHalfVelocity;
		SensorValue[enc] = 0;
		SensorValue[encB] = 0;
		//Calculate PID values
		difference = targetSpeed - velocity;

		//When the flywheel is running faster than it should, reduce power proportionally from the calibrated value
		if(difference < 0)
		{
			power = startSpeed + difference * flywheelKp;
		}
		//When it is shooting short range and almost at speed, reduce power from full speed to stop over shooting
		else if(difference < 2 && targetSpeed == shortTargetSpeed)
		{
			power = 90;
		}
		//When it is shooting at mid range and almost at speed, reduce power from full speed to stop over shooting
		{
			power = 100;
		}
		//Set full speed forwards if it is going slower than it should
		else if(difference > 0)
		{
			power = 127;
		}
		//It is running at the correct speed, set the power to the calibrated value
		else
		{
			power = startSpeed;
		}
		if(power < 0)
			power = 0;
		setFlywheel(power);
	}
}

EDIT: Here is a graph of our values we did when we first wrote it to try and tune it properly (rapid fire). We still aren’t sure about the weird spikes at the top! From it you can clearly see the recovery time (of when it was recorded), which helped us tune the rest of the robot to match it.

1 Like
#8

I am not a very clean coder, and this has a lot of unnecessary code in it. I used a weird approach to an extremely simple bang bang, which worked well enough for us (4bps mid and bar). Unfortunately we were not the best at driving it and failed at our states competition.

#pragma config(Sensor, dgtl11, wheelEncoder,   sensorQuadEncoder)
#pragma config(Motor,  port1,           Intake,        tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           TRL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           TL,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           BRL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           BL,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           TR,            tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           TLL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           BR,            tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           BLL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          Convey,        tmotorVex393_HBridge, openLoop, reversed)

int target=0;	//define target and rpm at global level so it can be passed between tasks
//int motorSpeed=0;
float rpm;
int lesser;
int higer;
int normal;
int currentEncoder;	//Define variables to use
int previousEncoder;
int deltaEncoder;
int currentTime;
int previousTime;
int deltaTime;
task speedCalculate();	//define task(s) to be used

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

void pre_auton()
{
	bStopTasksBetweenModes = true;
}
task autonomous()
{
	while(true){
		motor[TLL] = -91;
		motor[TRL] = 91;
		motor[BLL] = 91;
		motor[BRL] = -91;

		wait1Msec (3000);
		motor[TLL] = -91;
		motor[TRL] = 91;
		motor[BLL] = 91;
		motor[BRL] = -91;
		motor[Intake] = -125;
		motor[Convey] = 125;
		wait1Msec(12000);

	}

}

task usercontrol()
{
	//	SensorValue[greenLED]=0;
	//	startTask(speedControl);
	startTask(speedCalculate);

	while (true)
	{
//START INTAKE
		if (vexRT[Btn5UXmtr2] == 1){// Intake All On
			motor[Intake] = -125;
			motor[Convey] = 125;}

		if (vexRT[Btn7RXmtr2] == 1){// Intake All On
			motor[Intake] = 125;
			motor[Convey] = -125;}

		if (vexRT[Btn5DXmtr2] == 1){// Intake Stop
			motor[Intake] = 0;
			motor[Convey] = 0;}

		if (vexRT[Btn7UXmtr2] == 1){// Front Intake On
			motor[Intake] = -125;}

		if (vexRT[Btn7DXmtr2] == 1){// Back Intake On
			motor[Convey] = 125;}

		if (vexRT[Btn7LXmtr2] == 1){// Back Intake Off
			motor[Convey] = 0;}

		if (vexRT[Btn7RXmtr2] == 1){// Front Intake Off
			motor[Intake] = 0;}

//START FLYWHEEL, speeds in no particular order
		if (vexRT[Btn8RXmtr2] == 1){// midcourt
			target = 395;
			lesser = 50;
			higer = 125;
			normal = 75;
			motor[TLL] = -75;
			motor[TRL] = 75;
			motor[BLL] = 75;
			motor[BRL] = -75;
		}
		else	if (vexRT[Btn8DXmtr2] == 1){// BAR
			target = 320;
			lesser = 45;
			higer = 125;
			normal = 70;
			motor[TLL] = -70;
			motor[TRL] = 70;
			motor[BLL] = 70;
			motor[BRL] = -70;}
		else	if (vexRT[Btn8LXmtr2] == 1){// Midcourt 2
			target = 360;
			lesser = 50;
			higer = 125;
			normal = 73;
			motor[TLL] = -73;
			motor[TRL] = 73;
			motor[BLL] = 73;
			motor[BRL] = -73;}
		else if (vexRT[Btn8UXmtr2] == 1){// Launcher Preset Speed TILE(FULL BATTERY) - no bang bang, didn't have enough time to tune accuracy
			target = 450;
			lesser = 90;
			higer = 90;
			normal = 90;
			motor[TLL] = -90;
			motor[TRL] = 90;
			motor[BLL] = 90;
			motor[BRL] = -90;}
		else if (vexRT[Btn6UXmtr2] == 1){// Launcher Preset Speed TILE(Lower BATTERY)
			target = 450;
			lesser = 70;
			higer = 125;
			normal = 95;
			motor[TLL] = -95;
			motor[TRL] = 95;
			motor[BLL] = 95;
			motor[BRL] = -95;}
		else if (vexRT[Btn8R] == 1){// Launcher Preset Speed TILE (Lower BATTERY)- no bang bang, didn't have enough time to tune accuracy
			target = 450;
			lesser = 85;
			higer = 85;
			normal = 85;
			motor[TLL] = -85;
			motor[TRL] = 85;
			motor[BLL] = 85;
			motor[BRL] = -85;}
		else if (vexRT[Btn8L] == 1){// Launcher Preset Speed TILE(Lower BATTERY)- no bang bang, didn't have enough time to tune accuracy
			target = 450;
			lesser = 87;
			higer = 87;
			normal = 87;
			motor[TLL] = -87;
			motor[TRL] = 87;
			motor[BLL] = 87;
			motor[BRL] = -87;}
//Actual BANG BANG code, simplest form
		else	if (rpm > target) {
			motor[TLL] = -lesser;
			motor[TRL] = lesser;
			motor[BLL] = lesser;
			motor[BRL] = -lesser;
			}else if (rpm == target) {
			motor[TLL] = -normal;
			motor[TRL] = normal;
			motor[BLL] = normal;
			motor[BRL] = -normal;
			}else	if (rpm < target) {
			motor[TLL] = -higer;
			motor[TRL] = higer;
			motor[BLL] = higer;
			motor[BRL] = -higer;
		}
//START BASE PROGRAM - tank drive
		motor[TR] = vexRT[Ch3] - vexRT[Ch1] - vexRT[Ch4];
		motor[BR] = vexRT[Ch3] - vexRT[Ch1] + vexRT[Ch4];
		motor[TL] = vexRT[Ch3] + vexRT[Ch1] + vexRT[Ch4];
		motor[BL] = vexRT[Ch3] + vexRT[Ch1] - vexRT[Ch4];
	}

}

task speedCalculate() //self explanatory, same speed calculate can be used in other velocity control loops - for future reference
{

	while(true)
	{
		previousEncoder = SensorValue[wheelEncoder]; //Measure Encoder and time at beginning
		previousTime = nSysTime;
		wait1Msec(25); //Wait to measure changes
		currentEncoder = SensorValue[wheelEncoder]; //Measure Encoder and time after delay
		currentTime = nSysTime;
		deltaEncoder = currentEncoder - previousEncoder; //Calculate changes
		deltaTime = currentTime - previousTime;
		rpm = (deltaEncoder*60)*(1000/deltaTime)/360; //calculate rpm but converrt degress to rotations, ms to s, s to minutes
		wait1Msec(10); //Wait to allow calculations, as well as not hog CPU
	}
}
1 Like
#9

This is essentially a rehashing of my team’s original code reveal, but I’ll post here for the sake of sharing.

Team 2442A’s code is found in this repo. Everything there is dependent on a library I am actively developing called Bulldog Competition Includes (BCI) which can be found in this repo. Hopefully everything is neat and well commented, especially in BCI (everything you need to know to use BCI can be found in its various header files).

1 Like
#10

This is the Bang Bang velocity code my team had for worlds. Earlier in the year, I made a PID control code, but it was too slow to respond to 4bps fire rates (it was maxed out at 2) so I changed it (I’m not going to post the PID code).


#ifndef flywheel_c
#define flywheel_c

void Beep() {							//plays a beeping sound to help with ball loading times for full court shots
	if(time1[T3] > 697) {
		playTone(1500, 4);
		time1[T3] = 0;
	}
}

#define FULLCOURT 1
#define HALFCOURT 2
#define FRONTCOURT 3
#define OFF 0

int maxError,
		tv,
		divisor = 1,
		error,
		Full = 1;

float flywheelMP;

volatile float batMul;

void UpdatePIDValues(int pwr) {
	if(pwr == FULLCOURT) {						// update all the flywheel variables for full court shots
		tv = 155;
		maxError = 750;
		divisor = 16;
		batMul = 0.18;
		Full = 8;
	}
	if(pwr == HALFCOURT) {						// update all the flywheel variables for half court shots
		tv = 128;
		maxError = 280;
		divisor = 45;
		batMul = 0.3;
		Full = 1;
	}
	if(pwr == FRONTCOURT) {						// update all the flywheel variables for front court shots
		tv = 108;
		maxError = 90;
		divisor = 500;
		batMul = 0.36;
		Full = 1;
	}
	if(pwr == OFF) {									// just turn off the flywheel
		tv = 0;
		flywheelMP = 0;
		error = 0;
		SensorValue[En1] = 0;
		Full = 1;
	}
}

void UpdateFlywheel() {
	if(time1[T2] <= 50) return;
	if(tv != 0) {
		int batteryLevel = nAvgBatteryLevel / divisor;
		int capError = (maxError - batteryLevel) * batMul;	//caperror = power level motors have to be based on battery voltage to shoot at constant didtance regardless of voltage (9v & 7v shoot same distance)
		error = (tv - SensorValue[En1])/ Full;							//calculates error fo flywheel code. Full determines the magnitude of which the error will affect the code
		if(error > 75) flywheelMP = (flywheelMP +1) * 1.2 ; //gradually speeds up flywheel if it is > 75 error
		else if(error >= 3) flywheelMP = capError + 80;			//puts flywheel to full power when error is > 3
		else if(error == 1 || error == 2) flywheelMP = capError + 40;	//puts flywheel at medium speed faster than coast speed
		else if(error < 1 && error > -1) flywheelMP = capError + 15;	//puts flwheel at coast speed
		else if(error <= -1) flywheelMP = capError;										//puts flywheel below coast speed
	}
	motor[fly1] = flywheelMP;
	motor[fly2] = flywheelMP;
	motor[fly3] = flywheelMP;
	motor[fly4] = flywheelMP;
	SensorValue[En1] = 0;
	time1[T2] = 0;
}

void FlywheelController() {
	if(vexRT[Btn8U] == 1) UpdatePIDValues(FULLCOURT);
	if(vexRT[Btn8L] == 1) UpdatePIDValues(HALFCOURT);
	if(vexRT[Btn8R] == 1) UpdatePIDValues(FRONTCOURT);
	if(vexRT[Btn8D] == 1) UpdatePIDValues(OFF);
	UpdateFlywheel();
	if(tv == 155) Beep();
}

#endif
1 Like
#11

This is our hybrid bang-bang/TBH flywheel velocity control. It’s horribly and also sometimes not commented, and it’s also in EasyC, but here it is.

#include "Main.h"

void bangTBH ( void )
{
      time += 1 ;
      encOld = encNew ;
      encNew = GetQuadEncoder ( 7 , 8 ) ;
      encVelocity = encNew - encOld ;
      v1 = v2 ;
      v2 = v3 ;
      v3 = encVelocity ;
      vavg = (v1+v2+v3)/3 ;
      current = vavg ;
      errorOld = errorNew ;
      errorNew = target - current ;
      if ( pidOn )
      {
            command = trueCommand ;
            command += errorNew*kbtbh ;
            if ( sign(errorNew) != sign(errorOld) )
            {
                  commandTBH = .5 * (commandTBH + command); ;
                  command = commandTBH ;
            }
            if ( command > 127 )
            {
                  command = 127 ;
            }
            if ( command < 0 )
            {
                  command = 0 ;
            }
            if ( time > 1 && madRush == 0 ) // essentially commented here
            {
                  //madRush = 1 ;
                  //command = 127 ;
            }
            trueCommand = command ;
            if ( target - vavg > bangLambda )
            {
                  command = 127 ;
                  bang = 1 ;
            }
            if ( bang == 1 && target - vavg <= bangLambda )
            {
                  command = trueCommand ;
                  bang = 0 ;
            }
      }
      if ( Abs(errorOld) < lambda && Abs(errorNew) < lambda )
      {
            flywheelAtSpeed = 1 ;
      }
      else
      {
            flywheelAtSpeed = 0 ;
      }
}
1 Like
#12

Here’s a link to our GitHub repository that has all our code in it (MIT license): https://github.com/wa-robotics/2015-code

For our flywheels, we used a PI + constant controller with some special additions I added. The main one was that we used to P constants, one for normal operation and one for just after a ball was shot. The logic behind this was that after a ball was shot the higher P constant would kick in and give a boost to the motor power to help speed up recovery, but then, once the RPM approached the target again, the P constant dropped and so the motor power would drop a little to help prevent overshoot. This worked really well for us, especially for close and midfield shooting. We also had an integral active zone so the integral didn’t get adjusted right after a ball was shot.

We used the constant to speed up start up time; otherwise, it took about 10 seconds for the integral to get big enough to run the flywheel at the proper speed.

Our PIC controller: https://github.com/wa-robotics/2015-code/blob/master/Flywheel%20Robots/State/Global/Simple%20PID%20Controller.h (it’s very loosely based on the format of @jpearman’s TBH struct controller)

We actually did use a TBH controller for about half the season, but eventually we found that it couldn’t correct fully for the speed we wanted to shoot balls at, so we moved on to PID.

Our drivetrain code is pretty simple, although I wrote some slew rate code for driver control for one of our robots that had issues with the drivetrain burning out: https://github.com/wa-robotics/2015-code/blob/master/Flywheel%20Robots/Worlds/Alpha%20-%20Worlds.c#L369

1 Like
#13

Throughout the year my code had three flywheel control methods and some minor changes throughout driver control and autonomous that let us do some more fine work.
Originally we used slew rate (which we kept the stepper for all year) and some basic tuning. We had the ability to pulse a higher speed to aid recovery and let us fire faster.
Next came TBH. My original TBH was horribly clunky and had logic errors which caused us to have some issues with oscillations after about 4 shots. This is what I called TBH flawless, which was based off of Jpearman’s original TBH code, but added Feedforward to reduce overshoot and had motor protection through the original slew rate code:

void fSpeedControl(int omega, int step = 2)//Flywheel speed control based on motor values with slew rate.
{
	int currentSpeed = motor[launcher1];//get the current motor value.
	if(currentSpeed < omega)//if less than omega, increment slowly
	{
		motor[launcher1] = currentSpeed+step;
		motor[launcher2] = currentSpeed+step;
		motor[launcher3] = currentSpeed+step;//step allows us to change how quickly to spin up.
			motor[launcher4] = currentSpeed+step;
		if((omega - motor[launcher1])<=0)//when greater than or equal to, set equal to omega, and update current speed
		{
			motor[launcher1] = omega;
				motor[launcher2] = omega;
			motor[launcher3] = omega;
				motor[launcher4] = omega;
			currentSpeed = omega;
		}
	}
	if(currentSpeed > omega)//if greater than omega, decrease slowly
	{
		motor[launcher1] = currentSpeed-step;
			motor[launcher2] = currentSpeed-step;
		motor[launcher3] = currentSpeed-step;
			motor[launcher4] = currentSpeed-step;
		if((omega-motor[launcher1])>=0)//when less than or equal to, set equal to omega, and update current speed
		{
			motor[launcher1] = omega;
				motor[launcher2] = omega;
			motor[launcher3] = omega;
				motor[launcher4] = omega;
			currentSpeed = omega;
		}
	}
}
//These variables are global so we can see them in the debugger.
//They are used for the flywheel speed control and allow the
//flywheel to shoot the same distance despite battery voltage
//with the use of a TBH+Integral Velocity control algorithm.

int setRPM = 0;//The speed we set in drivermode or wherever else.
int error = 0;//The error in flywheel velocity (set - actual)
float speed = 0.0;//the speed that the motors are set to; gets error added to it to change speed;
//speed is seperate from the motor assignment, which is an integer, otherwise all decimals would be truncated and unless a massive error
//was generated, then the flywheel would always under or overshoot.
int InteRPM = 0;//The actual RPM as calculated by calcRPM task
int QuadRPM = 0;//this is RPM measurement from the Quadencoder directly geared down from the flywheel.
int filteredQuadRPM = 0;
int filteredInteRPM = 0;//An averaged rpm between 3 consecutive values which filters out the high frequency noise that comes from RPM measurement
//slows down actual measurement change, but reduces instability and increases accuracy.
int CompRPM = 0;//experimental complimentary filter for RPM. uses IMe in conjunction with Quad to provide better estimate
//for RPM over a given period, and reduces the innaccuracy of other filters.
task calcRPM()
{
	int count = 0;
	int prevIntegrated = 0;//previous postion of the encoder to calculate change in distance
	int prevQuadrature = 0;
	while(true)//run forever
	{
		prevIntegrated = nMotorEncoder[launcher3];//set previous position to position to have a reference point
		prevQuadrature = SensorValue[flywheel];
		delay(15);//wait for encoder to increase in value
		InteRPM = 35 * getMotorVelocity(launcher3);
		//InteRPM = (nMotorEncoder[launcher3]- prevIntegrated)/ 627.2 / 25.0 * 1000.0 * 60.0 * 35.0;//calculate the RPM in a float to save decimal places during calculation

		QuadRPM = (SensorValue[flywheel] - prevQuadrature)/ 360.0 / 15.0 * 1000 * 60 * 5.0;
		filteredInteRPM = (filteredInteRPM*5 + InteRPM)/6;//exponential moving average rpm and store in global variables for TBHFlywheel to see.
		filteredQuadRPM = (filteredQuadRPM*3 + QuadRPM)/4;
		CompRPM = (.85 * filteredQuadRPM) + (.15* InteRPM);
	}
}
int speedapprox = 0;
task TBHFlywheel()//Take Back Half controller with Integral accumulator for live adjustment
{//Used for the first time successfully after a series of breakthroughs using the graphing and analysis ability
	//through debug stream and tools in Excel.
	//Finally got RPM calculation right for both encoders, and also got TBH to work over snow break from 1-20 to 1-29

	//motor[launcher4] = 0;
	motor[launcher3] = 0;
	motor[launcher2] = 0;//stop flywheel ahead of time.
	motor[launcher1] = 0;

	nMotorEncoder[launcher3] = 0;
	SensorValue[flywheel] = 0;//clear flywheel encoder so prevent random data from causing error.
	startTask(calcRPM);//start RPM calculation

	float gain =0.001;// 0.00050;//0.00013;//0.00018;//gain for integral causes aggressive recovery of RPM after firing.

	float tbh = 0.0;//TBH is in a decimal to scale better and have lower gain.
	float power = 0.0;//power is multiplied by the max power to get the motor speed

	bool firstcross = true;//checking if the launcher needs to use the approximate speed
	int preverror = 0;//previous error for checking if the sign of the error has changed
	int slewrate = 10;//default slew rate for checking if the change in speed is too great for the launcher to handle
	int del = 30;//delay for loop.
	while(true)
	{
		if(CompRPM<0)
		{
			CompRPM = 0;
		}
		else
		{
			error = setRPM - filteredQuadRPM;//CompRPM;//calculate error; use quad encoder because it responds semi-accurate and quickly
		}
		if(setRPM  ==0)//prevent the low motor powers which can't break friction from running the motors when spinning down
		{
				speed = 0;
				//power = 0;
		}
		else if(error>0)
		{
			speed  = 105;
		}
		else if (error<0)
		{
			speed = 30;
		}

			motor[launcher4] = speed;
			motor[launcher3] = speed;//set the speed for the motors
			motor[launcher2] = speed;
			motor[launcher1] = speed;

		writeDebugStreamLine("%d,%4d,%4d,%4d,%3d,%4d",nSysTime,setRPM,QuadRPM,filteredInteRPM,speed,CompRPM);
		//^prints data to debug stream for graphing and analysis of the flywheel's performance.
		//enables easier tuning and much quicker adjustment, since it logs all flywheel activities.
		delay(del);
		if(setRPM ==0)//if spinning down, decrease the delay and decrease slew rate to make the flywheel spin down without breaking motors.
		{//edit: no longer need to be as safe on slowdown because of ratchets in flywheel gearboxes preventing the inertia from killing motors.
			del = 30;
			slewrate = 20;
		}
		else//otherwise a default slewrate and delay
		{
			del = 50;
			slewrate = 25;
		}
	}
}

The delay on spin down used to be larger, but after we added a ratcheting gear to the system, it was lowered to let us spin down quicker.
One cool trick we had was CompRPM, which used a complementary filter between an IME behind the ratchet, and a Quad in 5:1 step down from the flywheel in a moving average to get a more accurate reading. The IME read faster and was more stable, but was less accurate on the actual rate of the wheel. The Quad was very quick at reading and would catch the greatest drops in RPM within about 10rpm (i work at around 2300RPM), but had serious noise issues even when averaged. Combining portions of the two gave more accurate reading of drops in energy and more consistent readings when stabilizing. We ultimately switched finally to Bangbang for the extra fire rate, and did not use the CompRPM variable because the IME would sometimes not initialize and cause the RPM to read about 600 below target at 127 (We had 1:35).
NBNv5.5.zip (7 KB)

1 Like
#14

I did however have some cool autofire code for autonomous and driver. I was able to use a line follower to detect balls for firing when RPM is in range and for detecting how many shots were taken. This also allowed us to more effectively load our intake which held only 4 balls.
First, this is autofire, which would fire and exact number of shots within 2 shots of 64. It was based off of code tabor473 used to track shots fired from a linear puncher.

void autofire(int shots)
{
	 int shot =0;
	bool newball = true;
	bool oldball = false;
	bool firing = false;
	while(shot < shots)//run while less than n shots
		{
			if(newball && SensorValue[intake] >2500)
			{
				motor[intake] = 50;
				motor[intake2] = 110;
			}
			else if(newball && SensorValue[intake]<2500)
			{
				motor[intake] = 0;
				motor[intake2] = 0;
				newball = false;
				oldball = true;
			}

			if(oldball &&(error > -35&&error<0))
			{
				firing = true;
			}

			while(firing)
			{
				motor[intake] = 50;
				motor[intake2] = 110;
				if(SensorValue[intake] >2500)
				{
					firing = false;
					newball = true;
					oldball = false;
					motor[intake] = 0;
					motor[intake2] = 0;

					shot++;
				}
				wait1Msec(5);
			}

			delay(30);
		}
}

Next is the actual autofire in driver:

//Begins the if statement for controlling the intake on the joystick
		//Allows both to move at once, or each to move independently, or automatically for driverload spamming
		if(vexRT[Btn5DXmtr2] ==1)//||vexRT[Btn5U] == 1)//automatic ball feeding when speed is right
		{
			if(SensorValue[intake] > 2000 || ((error > -35&&error< 0)  && setRPM !=0&&vexRT[Btn5Uxmtr2] ==1))//if limit switch is not pressed, or error is low enough and
			{//flywheel is not set to not spin, feed into the flywheel
				motor[intake] = 70;
				motor[intake2] = 100;
			}
			else//otherwise run outer intake and stop inner
			{
				motor[intake] = 60;
				motor[intake2] = 0;
			}
		}
		else if(vexRT[Btn5DXmtr2] ==1)//||vexRT[Btn5U] == 1)//automatic ball feeding when speed is right
		{
			if(SensorValue[limit1] == 0)//if limit switch is not pressed, or error is low enough and
			{//flywheel is not set to not spin, feed into the flywheel
				motor[intake] = 70;
				motor[intake2] = 100;
			}
			else//otherwise run outer intake and stop inner
			{
				motor[intake] = 60;
				motor[intake2] = 0;
			}
		}
		else if(vexRT[Btn7UXmtr2] == 1)
		{
			motor[intake] = 80;//both at once in
			motor[intake2] = 100;
		}
		else if(vexRT[Btn7DXmtr2] == 1)
		{
			motor[intake] = -80;//both at once out
			motor[intake2] = -100;
		}
		else
		{
			motor[intake] = vexRT[Ch3Xmtr2];//both on independent joysticks to move separately
			motor[intake2] = vexRT[Ch2Xmtr2];
		}

This encases all of the intake control code we had so that we could more effectively manage shots and balls. we could use independent joysticks to intake, a force back or forward mode, or and auto load/ auto fire mode on the two left bumpers. holding Bottom bumper would load, and pressing top would launcher one, or holding would fire volleys when the shots was ready. We needed the force mode to fire faster in field and we could fire about 4bps with our trashy intake from half court.

There is some leftover stuff in the code that was not needed, but left in for reference (mostly in the RPM calculations and autonomous functions). The full file is in the first post if anyone wants the most recent version (bangbang included). I can also give the graphs from the flywheel data if anyone is interested (I have about 20-30 graphs from one flywheel alone trying to get reduced losses on shots).
Shoutout to @jpearman : I was the guy with 1575 who shook your hand outside the dome then ran off. Sorry I couldn’t speak longer, but we had lost a laptop with code for our robot that made it to the dome. I just wanted to say that your help on the forums with my code was very appreciated.

2 Likes
#15

This is the PID code 1224J used:

void iteratePID() {
   measured = nMotorEncoder[flywheel1];
   // Ignore strangely high error values
   if (abs(error) < 130) {
      error = target - measured;
   }

   integral += error;
   if (integral > integral_limit) {
      integral = integral_limit;
   } else if (integral < -integral_limit) {
      integral = -integral_limit;
   }

   derivative = error - error_prev;
   error_prev = error;

   motor[flywheel1] = motor[flywheel2] = motor[flywheel3] += KP*(error) + KI*(integral) + KD*(derivative);

   nMotorEncoder[flywheel1] = 0;
   wait1Msec(PIDdelay);
}

Pretty simple. I believe in the version we used at states, I set KI to 0 (no Integral). To compensate, I made the PID output add to the motor power value (instead of the motor power value being set directly to the PID output). This got rid of an oscillation issue we had, but just tuning KI more may have possibly fixed the oscillation too.

This is the function I used to give me data for tuning the PID constants. It would run the flywheel for 18 seconds, outputting timestamped data on the flywheel speed, power, etc.

void tunePID() {
   clearTimer(T1);
   target = 24;
   motor[intakeFront] = motor[intakeUp] = 127;
   while (true) {
   	iteratePID();

      // Uncomment the debug stream writer below if you want to output values that would
      // be human readable
      // writeDebugStreamLine("Time: %d ms. Measured: %d ticks. Error: %d Motor power: %d", time1[T1], measured, error, motor[flywheel1]);

      // Uncomment the debug stream writer below if you want to output values that would
      // be good for making a spreadsheet or graph
      //writeDebugStreamLine("%d,%d,%d", time1[T1], measured, motor[flywheel1]);

      // Automatically ramp flywheel motors down after 18 seconds
      if (time1[T1] > 18000) {
         while (true) {
   			if (motor[flywheel1] > 0) {
   				motor[flywheel1] = motor[flywheel2] = motor[flywheel3] -= 1;
   				wait1Msec(100);
   			}
   		}
      }
   }
}

Here’s one graph that came out of that data (made by importing the debug stream output into Excel):

This particular test was a bit odd because we found that the conveyors were feeding balls into the flywheel at the exact rate the flywheel would recover (you can see in the graph, as soon as the flywheel recovers, a ball is launched). Near the end you can see that the speed was actually overshooting the target value, so if the balls were spaced out more in our conveyor system, they probably wouldn’t be launched as consistently. Graphs were definitely helpful in finding quirks like this.

Is anyone interested in sharing their location tracking code? My team discussed it a lot early in the season, but we didn’t end up incorporating location tracking into any of our robots, and we’re curious to see what teams came up with.

2 Likes
#16

Here is the code team 3388 Foobar used,

https://github.com/team4719/NBN2016

Nothing super special. We used PID + a feed forward term for our flywheel, with a low pass filter on our velocity calculation. We also used QCC2’s pid controller library and @jpearmann’s gyro filtering library.

We have a file in our repo to track the net and then auto orient when a button was pressed it teleop. It kind of worked but I found like everyone else that error accumulates too quickly and it is much easier just to practice driving.

1 Like
#17

Here’s my code! My autonomous was particularly awesome as it didn’t miss of the preloads over the entire worlds competition. https://github.com/Madram/Reid16

1 Like
#18

Thank you to everyone who has responded in this thread, I think this is a first to have so many examples from world class robots. I will fork (or create a repo) everything into an organization as an archive for the future.

Keep them coming, lets see some Discobot code !!

1 Like
#19 

while(true){
   bool curse = false;
   bool tenballspersecond = true; 
}
1 Like
#20

Thanks, what’s your team number?

We were all a bit busy at that point, perhaps we can chat longer next year.

2 Likes
#21

Jpearman, we really liked using ur SmartMotorLib in our code this year, however, we only needed one function out of it, the Motor Current Function, which we only used for one motor. Could you help me extract just this one function out of the lib because it seems to be messing with our drive motors for some reason in auton.