void terminal_program_rotate(terminalapi_cmd_t *cmd_struct)
{
int32_t rotate_speed_md = 0;
if(!terminalapi_try_get_int32(cmd_struct, &rotate_speed_md)) return;
if(EC_SUCCESS != motion_rotate(rotate_speed_md, MOTION_ACCESS_TERMINAL))
{
terminalapi_print("Error! Cannot take control over motion!");
}
}
void Menu::turn()
{
gUserInterface.waitForHand();
playNote(2000, 200);
delay(1000);
enc_left_front_write(0);
enc_right_front_write(0);
enc_left_back_write(0);
enc_right_back_write(0);
Orientation::getInstance()->resetHeading();
motion_forward(180, 0, 0);
motion_rotate(180);
motion_forward(180, 0, 0);
motion_hold(100);
}
void motion_corner(float angle, float radius, float exit_speed) {
float errorRight, errorLeft;
float rightOutput, leftOutput;
float leftFraction, rightFraction;
float idealDistance, idealVelocity;
float forceLeftMotor, forceRightMotor;
float distance;
if (radius < 0) {
radius *= -1;
}
else if (radius == 0) {
motion_rotate(angle);
return;
}
if (exit_speed < 0) {
exit_speed *= -1;
}
distance = angle * 3.14159265359 * radius / 180;
if (distance < 0) {
distance *= -1;
}
elapsedMicros moveTime;
if (angle <= 0) {
leftFraction = (radius + MM_BETWEEN_WHEELS / 2) / radius;
rightFraction = (radius - MM_BETWEEN_WHEELS / 2) / radius;
}
else {
leftFraction = (radius - MM_BETWEEN_WHEELS / 2) / radius;
rightFraction = (radius + MM_BETWEEN_WHEELS / 2) / radius;
}
motionCalc motionCalc (distance, max_vel_corner, exit_speed, max_accel_corner, max_accel_corner);
PIDCorrectionCalculator* left_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
PIDCorrectionCalculator* right_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
// zero clock before move
moveTime = 0;
// execute motion
while (idealDistance != distance) {
//Run sensor protocol here. Sensor protocol should use encoder_left/right_write() to adjust for encoder error
idealDistance = motionCalc.idealDistance(moveTime);
idealVelocity = motionCalc.idealVelocity(moveTime);
errorLeft = enc_left_extrapolate() - idealDistance * rightFraction;
errorRight = enc_right_extrapolate() - idealDistance * leftFraction;
// use instantaneous velocity of each encoder to calculate what the ideal PWM would be
if (idealVelocity * leftFraction > 0) {
forceLeftMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * leftFraction + FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
else {
forceLeftMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * leftFraction - FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
if (idealVelocity * rightFraction > 0) {
forceRightMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * rightFraction + FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
else {
forceRightMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * rightFraction - FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
leftOutput = left_motor_output_calculator.Calculate(forceLeftMotor, idealVelocity * leftFraction);
rightOutput = right_motor_output_calculator.Calculate(forceRightMotor, idealVelocity * rightFraction);
//run PID loop here. new PID loop will add or subtract from a predetermined PWM value that was calculated with the motor curve and current ideal speed
// add PID error correction to ideal value
leftOutput += left_PID_calculator->Calculate(errorLeft);
rightOutput += right_PID_calculator->Calculate(errorRight);
// set motors to run at specified rate
motor_set(&motor_a, leftOutput);
motor_set(&motor_b, rightOutput);
}
enc_left_write(0);
enc_right_write(0);
}
