129 lines
3.1 KiB
C++
129 lines
3.1 KiB
C++
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include <SoftwareSerial.h>
|
|
#include "MeMCore.h"
|
|
|
|
double _wheelDiameter = 6.0;
|
|
double _distanceBetweenWheels = 11.5;
|
|
int _basicMotorSpeed = 100;
|
|
|
|
// Target position and angle
|
|
double _targetX = 20.0;
|
|
double _targetY = 15.0;
|
|
double _targetAngle = 0.0;
|
|
|
|
// Current position and angle
|
|
double _currentX = 0.0;
|
|
double _currentY = 0.0;
|
|
double _thetaCurrent = 0.0;
|
|
double _thetaBaseline = 0.0;
|
|
double _errorAngle = 0.0;
|
|
|
|
MeDCMotor _leftMotor(9);
|
|
MeDCMotor _rightMotor(10);
|
|
|
|
MeGyro _gyro;
|
|
|
|
void setup() {
|
|
Serial.begin();
|
|
|
|
// Set no speed for motors initially
|
|
_leftMotor.run((9) == M1 ? -(0) : (0));
|
|
_rightMotor.run((10) == M1 ? -(0) : (0));
|
|
|
|
// Initialize gyro
|
|
_gyro.begin();
|
|
|
|
// Wait for 2 seconds
|
|
delay(2000);
|
|
|
|
// Retrieve initial baseline for theta angle
|
|
_thetaBaseline = _gyro.getAngleZ();
|
|
}
|
|
|
|
void loop() {
|
|
// Calculate target angle based on target position
|
|
_targetAngle = calculateTargetAngle(_targetX, _targetY);
|
|
|
|
// Rotate to target angle
|
|
rotateToTargetAngle();
|
|
|
|
// Move forward for corresponding distance
|
|
moveForwardToTarget();
|
|
|
|
// Stop the robot after reaching the target
|
|
stopMotors();
|
|
}
|
|
|
|
void serialEvent() {
|
|
// No serial input used in this version
|
|
}
|
|
|
|
// Function to calculate target angle based on target position
|
|
double calculateTargetAngle(double targetX, double targetY) {
|
|
|
|
double deltaX = targetX - _currentX;
|
|
double deltaY = targetY - _currentY;
|
|
|
|
return atan2(deltaY, deltaX) * 180 / M_PI;
|
|
}
|
|
|
|
void move(int leftMotorSpeed, int rightMotorSpeed)
|
|
{
|
|
_leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
|
|
_rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
|
|
}
|
|
|
|
// Function to rotate the robot to the target angle
|
|
void rotateToTargetAngle() {
|
|
_errorAngle = _targetAngle - _thetaCurrent;
|
|
|
|
while (abs(_errorAngle) > 5) {
|
|
double w = _Kp_angle * _errorAngle;
|
|
double leftWheelSpeed = -_distanceBetweenWheels * w / _wheelDiameter;
|
|
double rightWheelSpeed = _distanceBetweenWheels * w / _wheelDiameter;
|
|
|
|
// crop wheel speed
|
|
if (leftWheelSpeed > 250)
|
|
leftWheelSpeed = 250;
|
|
if (rightWheelSpeed > 250)
|
|
rightWheelSpeed = 250;
|
|
|
|
// send the commands
|
|
move(rightWheelSpeed, leftWheelSpeed);
|
|
|
|
// retrieve new values from gyro
|
|
_gyro.update();
|
|
|
|
// get the theta angle (Rz)
|
|
_thetaCurrent = _gyro.getAngleZ() - _thetaBaseline;
|
|
_errorAngle = _targetAngle - _thetaCurrent;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Function to move forward for the corresponding distance
|
|
void moveForwardToTarget() {
|
|
// Calculate distance to target using Pythagorean theorem
|
|
double distanceToTarget = sqrt(pow(_targetX - _currentX, 2) + pow(_targetY - _currentY, 2));
|
|
|
|
// Calculate time to reach target based on speed = distance / time
|
|
double timeToTarget = distanceToTarget / _basicMotorSpeed; // time in seconds
|
|
|
|
// Move the robot forward
|
|
move(rightWheelSpeed, leftWheelSpeed);
|
|
|
|
// Wait for the calculated time
|
|
delay(timeToTarget * 1000); // delay function takes time in milliseconds
|
|
|
|
// Stop the motors
|
|
stopMotors();
|
|
}
|
|
|
|
// Function to stop the motors
|
|
void stopMotors() {
|
|
_leftMotor.stop();
|
|
_rightMotor.stop();
|
|
}
|