RPM Calculation Help

I’m trying to write an implementation of TBH for my flywheel, however it is not working right. It never reaches the correct speed it is supposed to be at, and I determined this was because the RPM calculation was incorrect. Also, the current and former position variables are always the same value as each other. Could someone inspect the algorithm to help figure out why the RPM calculation always returns “0”.

int fRPM = 0;
int Ferror = 0;
float trueRPM = 0;
int position = 0;
int prevposition = 0;
int Fpreverror =0;
int tbh = 0;
float speed = 0;
task TBHFlywheel()
	float gain = .00005;
	SensorValue[flywheel] = 0;
	bool firstcross = true;
		position  = SensorValue[flywheel];
		trueRPM = (position -  prevposition)/360 / 50 * 1000 * 60 * 35;
		Ferror = fRPM - trueRPM;
		prevposition = position;
		speed = speed + (Ferror *gain);

		if((Ferror>0)!= (Fpreverror>0))
				speed = .5*(speed +tbh);
				tbh = speed;
				Fpreverror = Ferror;
				//firstcross = false;
		if(speed > 120)
			speed = 120;
		if(speed <0)
			speed = 0;

		if(fRPM == 0)
		firstcross = true;


fRPM is the value set in driver control to control the speed. The Quad-Encoder does work and counts positive, and is directly connected to a high strength 393.

OK i’m not realy sure what is wrong with your algorithm, but I have a couple pieces of advice

  1. separate your velocity calculation into a separate task, and there is a simpler way to preform the velocity calculation

here is how I recommend you do it

task velocity(){
int Enc;
int Dtime;
int RPM;
Enc = SensorValue[Encoder];
RPM = (Enc/360) * (1000/Dtime) * 60;
SensorValue[Encoder] = 0;

Why are you calculating RPM in the first place? Seem like a lot of extra code. Why not just use the encoder value as the target?

What does it say in the debugger?

Why is the true RPM a float and the rest integers? Can it be an integer since its use is cast back to an integer dropping any decimals?

Why do you have two waits in there totaling 50ms? Why not wait just in one spot?

What is the 35 in the RPM calcuation?

360 ticks/rev
50 ms wait
1000 ms/sec
60 sec/min
35 ???

I separated RPM into a new task, and swapped to an IME and this got TBH working. The normal encoders were not counting above 100 (all 3 of them) so we decided to try an IME and it worked on the first try.
The 35 was for the total gear ratio. This gives the RPM of the flywheel, and not the axle attached to it, which in this case would be spinning at 100 rpm without the conversion.
thanks for the help guys! Now for some more tuning and a little spooky code stuff.