void checkForKeyPresses()
{
/* Robot control keybinds
* Up arrow: + speed_change for both wheels [/\]
* Down arrow: - speed_change for both wheels [\/]
* Left arrow: + speedL_change for left wheel [<-]
* Shift + Left arrow: - speedL_change for left wheel [S + <-]
* Right arrow: + speedR_change for right wheel [->]
* Shift + Right arrow: - speedR_change for right wheel [S + ->]
* Space: Reset all speeds to 0 and brake [_]
*/
if (Input.GetKeyDown(KeyCode.UpArrow))
{
MC_R.addSpeed(speed_change);
MC_L.addSpeed(speed_change);
}
else if (Input.GetKeyDown(KeyCode.DownArrow))
{
MC_R.addSpeed(-speed_change);
MC_L.addSpeed(-speed_change);
}
else if (Input.GetKeyDown(KeyCode.Space))
{
MC_R.setSpeed(0);
MC_L.setSpeed(0);
MC_R.setBrake(true);
MC_L.setBrake(true);
}
else if (Input.GetKeyDown(KeyCode.RightArrow) && !Input.GetKey(KeyCode.LeftShift))
{
MC_R.addSpeed(speedR_change);
}
else if (Input.GetKeyDown(KeyCode.LeftArrow) && !Input.GetKey(KeyCode.LeftShift))
{
MC_L.addSpeed(speedL_change);
}
else if (Input.GetKeyDown(KeyCode.RightArrow) && Input.GetKey(KeyCode.LeftShift))
{
MC_R.addSpeed(-speedR_change);
}
else if (Input.GetKeyDown(KeyCode.LeftArrow) && Input.GetKey(KeyCode.LeftShift))
{
MC_L.addSpeed(-speedL_change);
}
if (!Input.GetKey(KeyCode.Space))
{
MC_R.setBrake(false);
MC_L.setBrake(false);
}
}
void drive()
{
if (sensorFront.getSensorReady())
{
if (!arrived)
{
distance = sensorImpulseToDistance(sensorFront.getHitTicks());
e = distance - distance_wanted;
u = Kp * e + Kd * (e - e_prev);
}
else
{
u = 0;
movFinished = true;
}
if (e <= e_expected && e >= -e_expected)
{
arrived = true;
}
if (u == 0)
{
motorLeft.setBrake(true);
motorRight.setBrake(true);
arrived = true;
}
else
{
motorLeft.setBrake(false);
motorRight.setBrake(false);
}
motorLeft.setSpeedPercent((float)u);
motorRight.setSpeedPercent((float)u);
e_prev = e;
}
}