RobotCProject.c

#pragma config(Sensor, S2,     gyroSensor,     sensorEV3_Gyro)
#pragma config(Motor,  motorB,          leftMotor,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          rightMotor,    tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

float wheelCircumfrence = 17.75; // cm
float forwardDistance = 50;

float errorArray[3] = {0,0,0};
float powerArray[2] = {0,0};
float samplingTime = 0.1; // TODO Get this value

float absVal(float n)
{
	if(n < 0)
	{
		return n * -1;
	}
	return n;
}

float controllerFP(float error,float kF,float kP)
{ // kF=10 and kP=1
	return error * kP + kF * error/absVal(error); //kF is applied in the unit vector direction of the error
}

float controllerPID(float error, float kP, float kI, float kD)
{
	float power = 0;
	float a = kP + kI * samplingTime / 2 + kD / samplingTime;
	float b = -kP + kI * samplingTime / 2 - 2 * kD / samplingTime;
	float c = kD / samplingTime;
	errorArray[0] = errorArray[1]; // Errors get shifted to lower indexes
	errorArray[1] = errorArray [2];
	errorArray[2] = error; // Newest Error in index two
	powerArray[1] = powerArray[0] + a * errorArray[2] + b * errorArray[1] + c * errorArray[0];
	power = powerArray[1];
	powerArray[0] = powerArray[1];
	return power;
}

void turnLeft()
{
	resetGyro(gyroSensor);
	float kF = 10; // TODO tune all of these
	float kP = 0.8;
	float kI = 0.05;
	float kD = 0.01;
	float threshold = 1;
	float error = 0;
	float powers = 0; //Max can be 100
	float endAngle = -90; //90 degrees to the left
	while(absVal(getGyroHeading(gyroSensor)-(endAngle))>threshold)
	{
		error = getGyroHeading(gyroSensor)-(endAngle);
		powers = controllerPID(error, kP, kI, kD);
		if(absVal(powers) > 100) // scale the power to be in the range of a 100 to -100
		{
			powers = 100*powers/abs(powers);
		}
		setMotorSpeed(leftMotor, -powers);
		setMotorSpeed(rightMotor, powers);
	}
}

void forwardMove(float distance) // In centimeters
{
	float degrees = distance * 360 / wheelCircumfrence;
	moveMotorTarget(leftMotor, degrees,100);
	moveMotorTarget(rightMotor, degrees,100);
	waitUntilMotorStop(leftMotor);
	waitUntilMotorStop(rightMotor);
}

task main()
{ //Move in a box
	/*
	forward
	left turn
	forward
	left turn
	forward
	left turn
	forward
	*/
	resetGyro(gyroSensor);

	forwardMove(forwardDistance);
	turnLeft();
	forwardMove(forwardDistance);
	turnLeft();
	forwardMove(forwardDistance);
	turnLeft();
	forwardMove(forwardDistance);
}