FrostBot: Dessert-Frosting Robot

const float MAXPISTON = 580;

const float PINIONRADIUS = 0.85;

// RADIUS DISPLAY AND PICKER

float getRadius() 

{

    const int NUM_CHOICES = 5;

    float choices[NUM_CHOICES] = {2.0, 2.5, 3.0, 3.5, 4.0};

    int select = 0;

    float radius = 0;

    for (int index = 0; index < NUM_CHOICES; index++) // displays all choices

    {

        displayString(index + 2, "Radius of %.1f", choices[index]);

    }

    while (radius == 0) 

    {

        if (getButtonPress(buttonUp) == 1) 

        {

            while (getButtonPress(buttonUp)) {}

            select--;

            if (select < 0) // used if select is before first option

                select = 0;

        }

        else if (getButtonPress(buttonDown) == 1) 

        {

            while (getButtonPress(buttonDown)) {}

            select++;

            if (select > NUM_CHOICES - 1) // used if select is past last option

                select = NUM_CHOICES - 1;

        }

        displayString(select + 2, "Radius of %.1f <-", choices[select]); // replaces selected item with line + cursor

        for (int index = 0; index < NUM_CHOICES; index++) 

        {

            if (index != select)

                displayString(index + 2, "Radius of %.1f", choices[index]); // fills in all other lines to remove previous cursor

        }

        if (getButtonPress(buttonEnter) == 1) // selection is approved by pressing enter

        {

            radius = choices[select];

            eraseDisplay();

            displayString(4, "Radius of %.2f was selected", radius);

            wait1Msec(500);

            return radius;

        }

    }

    return 0;

}

// CONFIGURE ALL SENSORS

void configureAllSensors() 

{

    SensorType[S1] = sensorEV3_Touch;

    SensorType[S4] = sensorEV3_Gyro;

    wait1Msec(50);

    SensorMode[S4] = modeEV3Gyro_Calibration;

    wait1Msec(100);

    SensorMode[S4] = modeEV3Gyro_RateAndAngle;

    wait1Msec(50);

}

// CHECK SECURITY

void checkSecurity(bool & isGoing) // if touch is pressed, turn isGoing false

{

    if (SensorValue[S1] == 1)

        isGoing = false;

}

// WAIT FOR SPECIFIED BUTTON PRESS WHILE CHECKING SECURITY

bool waitButton(TEV3Buttons button_name, bool & isGoing) 

{

    while (!getButtonPress(button_name))

    {

        checkSecurity(isGoing);

    }

    while (getButtonPress(button_name))

    {

        checkSecurity(isGoing);

    }

    return true;

}

// PISTON STOP

void pistonStop() 

{

    if (nMotorEncoder[motorB] >= MAXPISTON) 

    {

        displayString(4, "Time for a refill!");

        displayString(5, "Press touch when ready");

        motor[motorB] = motor[motorA] = motor[motorD] = 0;

        wait1Msec(50);

        motor[motorB] = -7; // retract piston

        while (nMotorEncoder[motorB] > 10) {}

        motor[motorB] = 0;

        while (SensorValue[S1] == 0) {}

        while (SensorValue[S1] == 1) {}

    }

}

// CONTROL PISTON WITH GYRO

void gyroControl(bool &isGoing) 

{

    while (!isGoing) 

    {

        int gyroAngle = SensorValue[S4];

        // Scale the gyro angle to adjust motor response

        int motorSpeed = gyroAngle / 2;

        // Set motor B speed based on gyro angle

        if (gyroAngle <= -5 || gyroAngle >= 5)

            motor[motorB] = motorSpeed;

        else

            motor[motorB] = 0;

        displayString(4, "Set piston height w/ gyro &");

        displayString(5, "Press touch button to start!");

        while (SensorValue[S1] == 1) // waits for touch to start

            isGoing = true;

    }

    eraseDisplay();

}

// EXECUTE ICING

void executeIcing(bool & isGoing, float radius) 

{

    eraseDisplay();

    do

    {

        isGoing = false;

        gyroControl(isGoing);

        wait1Msec(500);

        motor[motorB] = 4;

        time1[T1] = 0;

        while (isGoing == 1 && time1[T1] < 800) 

        {

            checkSecurity(isGoing);

        }

        motor[motorA] = 2;

        motor[motorD] = 7;

        while (nMotorEncoder[motorB] < MAXPISTON && isGoing == 1 && fabs(nMotorEncoder[motorA]) < (radius * 180) / (PI * PINIONRADIUS)) 

        {

            checkSecurity(isGoing);

        }

        motor[motorA] = motor[motorD] = motor[motorB] = 0;

        pistonStop();

        wait1Msec(50);

    } while (isGoing == 1 && fabs(nMotorEncoder[motorA]) < (radius * 180) / (PI * PINIONRADIUS));

}

// RESET ICING

void resetIcing(bool & isGoing) 

{

    motor[motorA] = motor[motorB] = motor[motorD] = 0;

    eraseDisplay();

    displayString(3, "Enjoy your cookie and");

    displayTextLine(4, "Press any button to reset");

    waitButton(buttonAny, isGoing);

    // lift icing dispenser

    motor[motorB] = -7;

    while (nMotorEncoder[motorB] >= 10) {}

    motor[motorB] = 0;

    motor[motorA] = -5;

    while (nMotorEncoder[motorA] >= 0) {}

    motor[motorA] = 0;

    wait1Msec(1000);

}

// TASK MAIN

task main() 

{

    configureAllSensors();

    float radius = getRadius();

    eraseDisplay();

    bool isGoing = false;

    executeIcing(isGoing, radius);

    resetIcing(isGoing);

}