100 lines
3.2 KiB
C++
100 lines
3.2 KiB
C++
#include "Arduino.h"
|
|
#include "Wire.h"
|
|
#include "MeMCore.h"
|
|
|
|
MeGyro gyro(PORT_1); // Initialize the gyro sensor on port 1
|
|
MeDCMotor _leftMotor(9);
|
|
MeDCMotor _rightMotor(10);
|
|
// Define constants
|
|
const int targetSpeed = 50; // Speed for translation movements
|
|
const int wheelDistanceBetweenWheels = 11; // Distance between the wheels in centimeters
|
|
const int wheelRadius = 3; // Radius of the wheels in centimeters
|
|
|
|
// Function to calculate wheel speeds based on translational and rotational speeds
|
|
void calculateWheelSpeeds(float v, float theta_dot, float &vL, float &vR) {
|
|
vL = v/wheelRadius - (wheelDistanceBetweenWheels*theta_dot)/wheelRadius*2;
|
|
vR = v/wheelRadius + (wheelDistanceBetweenWheels*theta_dot)/wheelRadius*2;
|
|
}
|
|
|
|
// Function to perform rotation movement
|
|
void rotate(float targetAngle, float v) {
|
|
const float KP = 0.5; // Proportional control gain
|
|
const float MIN_SPEED = 10; // Minimum rotation speed
|
|
|
|
float currentAngle;
|
|
float error;
|
|
float rotationSpeed;
|
|
|
|
do {
|
|
// Get current angle
|
|
gyro.update();
|
|
currentAngle = gyro.getAngleZ();
|
|
Serial.print("Current angle :");
|
|
Serial.print(currentAngle);
|
|
Serial.print(" Objective :");
|
|
Serial.println(targetAngle);
|
|
|
|
// Calculate error
|
|
error = targetAngle - currentAngle;
|
|
while (error > 180) error -= 360;
|
|
while (error < -180) error += 360;
|
|
|
|
// Calculate rotation speed using proportional control
|
|
rotationSpeed = KP * error;
|
|
|
|
// Constrain rotation speed to ensure it doesn't exceed the minimum speed
|
|
rotationSpeed = constrain(rotationSpeed, -MIN_SPEED, MIN_SPEED);
|
|
|
|
// Calculate wheel speeds based on rotation speed
|
|
float vL, vR;
|
|
calculateWheelSpeeds(v, rotationSpeed, vL, vR);
|
|
|
|
// Apply speeds to motors
|
|
_leftMotor.run(-vL);
|
|
_rightMotor.run(vR);
|
|
} while (abs(error) > 5); // Continue rotation until error is within acceptable threshold
|
|
}
|
|
|
|
|
|
// Function to perform translation movement
|
|
void translate(float distance) {
|
|
// Calculate time required to travel given distance at target speed
|
|
float time = distance / targetSpeed;
|
|
_leftMotor.run(-targetSpeed);
|
|
_rightMotor.run(targetSpeed);
|
|
Serial.print("Time calculated for next segment : ");
|
|
Serial.println(time*1000);
|
|
delay(time * 1000); // Convert time to milliseconds
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(9600); // Initialize serial communication
|
|
gyro.begin(); // Initialize the gyro sensor
|
|
_leftMotor.run(0);
|
|
_rightMotor.run(0);
|
|
}
|
|
|
|
void loop() {
|
|
// Array containing the 2D positions of the 4 corners of the square
|
|
float targetPoints[4][2] = {{0, 0}, {10, 0}, {10, 10}, {0, 10}};
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
float targetX = targetPoints[i][0];
|
|
float targetY = targetPoints[i][1];
|
|
|
|
// Calculate rotation angle towards target point
|
|
float currentX = 0; // Assume starting from origin
|
|
float currentY = 0; // Assume starting from origin
|
|
float targetAngle = atan2(targetY - currentY, targetX - currentX) * 180 / PI;
|
|
|
|
// Perform rotation towards target point
|
|
rotate(targetAngle, targetSpeed);
|
|
|
|
// Calculate distance to target point
|
|
float distance = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2));
|
|
|
|
// Perform translation towards target point
|
|
translate(distance);
|
|
}
|
|
}
|