Forward for Distance, Smooth Accelleration and Decelleration Code for LEGO EV3 in ROBOTC (elliptical algorithm)

Our Robotics Team was having trouble with the robots starting and stopping so quickly that there was wheel slippage. This was effecting our gameplay and likely cost us first place last year. This is the code that was generated to help alliviate that issue. I would love to incorportate a variety of the PID algorithm to this elliptical ‘P’ algorithm. Any thought leave a comment below on our facebook page. See also: Using the Sonar Sensor


#pragma config(Sensor, S1, touch, sensorEV3_Touch)
#pragma config(Sensor, S2, gyro, sensorEV3_Gyro)
#pragma config(Sensor, S3, color, sensorEV3_Color)
#pragma config(Sensor, S4, sonic, sensorEV3_Ultrasonic)
#pragma config(Motor, motorB, left, tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor, motorC, right, tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

//Standard dimensions for program
float wheelDiameter = 5.6; // standard EV3 Wheel Diameter in cm.

void resetMotors() {
// Name: resetMotors
// Author: Andy Hatfield
// Desc: Resets the motor encoder for left and right (generally B and C respectively
resetMotorEncoder(left);
resetMotorEncoder(right);
}

void brake () {
// Name: brake
// Author: Andy Hatfield
// Desc: Stops the wheels for a duration of 1/100 sec
setMotorSync(left, right, 0, 0);
sleep(10);
}

void moveStraight(float pwr, float dist) // pwr is max power, dist is in cm and needs wheelDiameter defined globally.
{
// Name: brake
// Author: Andy Hatfield
// Desc: This moveStraight has an elliptical accelleration curve, starts easy and ends easy preventing wheel slippage due to
// starting and stopping too quickly. Positive pwr is forward, negative pwr is backwards
// Notes:
// The distance is pretty precise at higher distances, at short distances it is not very precise though.
// Higher powers result in higher overshooting.

float deg = dist / (3.1415 * wheelDiameter) * 360;
float lPwr; // a local Power
float center = deg / 2;
float x;
int dir = 1;
if (pwr < 0) { dir = -1; pwr = -pwr; } resetMotors(); while (abs(getMotorEncoder(left)) < deg) { x = abs(getMotorEncoder(left)); lPwr = 2 + pwr * sqrt(1 - (1 / pow(center, 2)) * pow(x - center, 2)); setMotorSync(left, right, 0, dir * lPwr); } brake(); } task main() { moveStraight(50, 3 * 5.6 * 3.1415); }

Leave a Reply

Your email address will not be published. Required fields are marked *

*