Controlled Accelleration and Decelleration


#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)

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

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

// moveStraight
// Author: Andy Hatfield
// Desc: Will cause the robot to move forward with a smooth accelleration and decelleration.
// pwr - motor power
// deg - degrees of motor rotation
// smooth - is a function of pwr * 4 / 3
void moveStraight (float pwr, float deg) { //pwr is power -100 to 100, rtn is rtnation in degrees
float smooth = pwr * 4 / 3;
int dir = 1;
if (pwr < 0) { dir = -1; pwr = -pwr; } resetMotors(); //gradual start up while (abs(nMotorEncoder[left]) < smooth) { setMotorSync(left, right, 0, dir * (pwr * abs(nMotorEncoder[left]) / smooth + 5)); // Power * (encoder/180) } // go until half a turn away from stop int mDeg = deg - smooth; setMotorSyncEncoder(left, right, 0, mDeg, dir * pwr); while(abs(getMotorEncoder(left)) < mDeg) { } // gradual stop for the last smooth degrees while (abs(nMotorEncoder[left]) < deg) { setMotorSync(left, right, 0, dir * (pwr * (deg - abs(nMotorEncoder[left])) / smooth + 5)); } brake(); } task main() { int j = 5; for(int i=1;i<=j;i++){ eraseDisplay(); displayCenteredBigTextLine(1, "Going Forward"); displayCenteredBigTextLine(5, "%d of %d times", i, j); moveStraight(90, 360); // forward 2 rotation eraseDisplay(); displayCenteredBigTextLine(1, "Going Backward"); displayCenteredBigTextLine(5, "%d of %d times", i, j); moveStraight(-90, 360); // forward 2 rotation } eraseDisplay(); displayCenteredBigTextLine(1, "All Done"); displayCenteredBigTextLine(5, "Thanks"); sleep(3000); }

Leave a Reply

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

*