143 lines
3.3 KiB
C++
143 lines
3.3 KiB
C++
/**
|
|
* @file control.ino
|
|
* @brief Description: this code drives the mbot robot to a target location (x, y, theta) using the gyro data.
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include <SoftwareSerial.h>
|
|
#include "MeMCore.h"
|
|
|
|
|
|
// declare motors
|
|
MeDCMotor _leftMotor(9);
|
|
MeDCMotor _rightMotor(10);
|
|
|
|
// declare gyro
|
|
MeGyro _gyro;
|
|
|
|
|
|
|
|
// robot characteristics
|
|
double const _wheelDiameter = 6.0; //cm
|
|
double const _distanceBetweenWheels = 11.5; //cm
|
|
int _basicMotorSpeed = 200;
|
|
|
|
|
|
// Vars to change setup
|
|
double _Kp_angle = -5.0;
|
|
double _targetAngle = 30;
|
|
|
|
// position, baseline...
|
|
double _thetaCurrent = 0.0;
|
|
double _thetaBaseline;
|
|
double _errorAngle = 0.0; // in deg
|
|
|
|
// Define the four corners of the square
|
|
double corners[4][2] = {{0, 0}, {0, 1}, {1, 1}, {1, 0}};
|
|
|
|
// Initialize the current position to the first corner
|
|
float current_x;
|
|
float current_y;
|
|
|
|
|
|
void setup()
|
|
{
|
|
// initialize the serial port
|
|
Serial.begin(9600);
|
|
|
|
// set no speed for left and right motors
|
|
_leftMotor.run((9)==M1?-(0):(0));
|
|
_rightMotor.run((10)==M1?-(0):(0));
|
|
|
|
// initialize the gyro
|
|
_gyro.begin();
|
|
|
|
// wait for 2 seconds
|
|
delay(2000);
|
|
|
|
// retrieve new values from gyro
|
|
_gyro.update();
|
|
|
|
// record the baseline for the theta angle (Rz)
|
|
_thetaBaseline = _gyro.getAngleZ();
|
|
|
|
current_x = corners[0][0];
|
|
current_y = corners[0][1]
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
|
|
// Loop over every positions
|
|
for (int i=0; i<sizeof(corners); i++)
|
|
{
|
|
|
|
// calculate the target angle and distance
|
|
|
|
// dist
|
|
float dx = corners[i][0] - current_x;
|
|
float dy = corners[i][1] - current_y;
|
|
|
|
float dist = sqrt(pow(dx, 2) + pow(dx, 2));
|
|
_targetAngle = atan2(dy, dx) * 180 / PI;
|
|
|
|
// Calculate the time required to travel the distance at the given speed
|
|
int timeS = ((dist)/(PI*2*0.03*(_basicMotorSpeed/60)));
|
|
|
|
// Rotate the robot towards the target angle until the error is within 2 degrees
|
|
while (fabs(_thetaCurrent - _targetAngle) > 2) {
|
|
|
|
// retrieve new values from gyro
|
|
_gyro.update();
|
|
|
|
// get the theta angle (Rz)
|
|
_thetaCurrent = _gyro.getAngleZ() - _thetaBaseline;
|
|
_errorAngle = _targetAngle - _thetaCurrent;
|
|
|
|
// set _errorAngle to be btw [-180 and 180]
|
|
if (fabs(_errorAngle) > 180) {
|
|
_errorAngle = (-1) * _errorAngle/fabs(_errorAngle) * 360 + _errorAngle;
|
|
}
|
|
|
|
// determine the command (proportionnal controller) i.e. angular velocity
|
|
double v = _distanceBetweenWheels * _Kp_angle * _errorAngle / (_wheelDiameter) ;
|
|
|
|
// compute the command
|
|
double leftWheelSpeed = -v ;
|
|
double rightWheelSpeed = v;
|
|
|
|
// crop wheel speed to 250
|
|
leftWheelSpeed = min(250, leftWheelSpeed);
|
|
rightWheelSpeed = min(250, rightWheelSpeed);
|
|
|
|
// send the commands to move the robot
|
|
move(rightWheelSpeed, leftWheelSpeed);
|
|
}
|
|
|
|
// Stop the robot
|
|
move(_basicMotorSpeed, _basicMotorSpeed);
|
|
delay(timeS*1000);
|
|
|
|
// Update the current position and move to the next corner
|
|
current_x = target_x;
|
|
current_y = target_y;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// Function to move the robot
|
|
void move(int leftMotorSpeed, int rightMotorSpeed)
|
|
{
|
|
_leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
|
|
_rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
|
|
}
|
|
|
|
// Function to stop the robot
|
|
void stop()
|
|
{
|
|
_leftMotor.stop();
|
|
_rightMotor.stop();
|
|
}
|