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!");	
		}
	}
예제 #2
0
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);
}
예제 #3
0
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);
}