gauthier.bassereau
/
IntroRoboticsLab2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: gauthier.bassereau:master
Branches Tags
gauthier.bassereau:master
gauthier.bassereau:develop
..
compare: gauthier.bassereau:develop
Branches Tags
gauthier.bassereau:master
gauthier.bassereau:develop

No commits in common. "master" and "develop" have entirely different histories.
master ... develop

9 changed files with 0 additions and 477 deletions
Show Stats Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

128
Control/Control.ino
View File

@ -1,128 +0,0 @@
#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();
}

BIN
IntroductionToRobotics_Lab2_LouisGraz_GauthierBassereau.pdf
View File

Binary file not shown.

99
autonom/autonom.ino
View File

@ -1,99 +0,0 @@
#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);
}
}

28
gyro_test/gyro.ino
View File

@ -1,28 +0,0 @@
#include "Arduino.h"
#include "Wire.h"
#include "MeMCore.h"
MeGyro gyro(PORT_1);
void setup() {
Serial.begin(9600); // Initialize serial communication
gyro.begin(); // Initialize the gyro sensor
}
void loop() {
gyro.update();
// Read gyro data
double anglex = gyro.getAngleX();
double angley = gyro.getAngleY();
double anglez = gyro.getAngleZ();
// Print gyro data
Serial.print("anglex:");
Serial.print(anglex);
Serial.print(",angley:");
Serial.print(angley);
Serial.print(",anglez:");
Serial.println(anglez);
delay(500); // Delay for stability, adjust as needed
}

52
roomba/main/main.ino
View File

@ -1,52 +0,0 @@
#include "Arduino.h"
#include "Wire.h"
#include "MeMCore.h"
// Declare 1 variable of type MeUltrasonicSensor at PORT_3
MeUltrasonicSensor sensor(PORT_3);
int speed = 100;
MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);
bool goagain = false;
// Define the number of iterations required to turn for 30 degrees
// This value may need to be adjusted based on the specific motor and robot configuration
#define TURN_ITERATIONS 50
void setup() {
// Initialize the serial port at 9600 baud
Serial.begin(9600);
// put your setup code here, to run once:
_leftMotor.run(0);
_rightMotor.run(0);
}
void loop() {
float distance = sensor.distanceCm();
// Use parentheses to enclose the condition
if (distance <= 15 or goagain == true) {
goagain = false;
// Add a for loop to turn for 30 degrees
for (int i = 0; i < TURN_ITERATIONS; i++) {
_leftMotor.run(speed);
_rightMotor.run(speed);
delay(10);
}
// Stop the motors after turning
_leftMotor.run(0);
_rightMotor.run(0);
// Check the distance again
distance = sensor.distanceCm();
if (distance < 90) {
goagain = true;
}
} else {
_leftMotor.run(-speed);
_rightMotor.run(speed);
}
// Add a small delay to avoid continuous motor changes
delay(10);
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
}

146
servoing/servoing.ino
View File

@ -1,146 +0,0 @@
/**
* @file control.ino
* @author Guillaume Gibert
* @version V0.1
* @date 2021/04/18
* @brief Description: this code drives the mbot robot to a target location (x, y, theta) using the gyro data.
*
* Function List:
* 1. void control::move(int, int)
* 2. void control::stop(void)
*
*/
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include "MeMCore.h"
// Vars to change setup
double _Kp_angle = -5.0;
double _targetAngle = 90;
const long int _baudRate = 115200;
// robot characteristics
double _wheelDiameter = 6.0; //cm
double _distanceBetweenWheels = 11.5; //cm
int _basicMotorSpeed = 100;
// internal global variables
String _inputString = ""; // a String to hold incoming data
bool _stringComplete = false; // whether the string is complete
// declare motors
MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);
// declare gyro
MeGyro _gyro;
// position, baseline...
double _thetaCurrent = 0.0;
double _thetaBaseline = 0.0;
double _errorAngle = 0.0; // in deg
void setup()
{
// initialize the serial port
Serial.begin(_baudRate);
// 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();
}
void loop()
{
// checks if an update of Kp value is available
if (_stringComplete)
{
// updates the Kp value
_Kp_angle = _inputString.toDouble();
// resets the input string to accept new input from the serial port
_inputString = "";
_stringComplete = false;
}
// adjust the angle
_errorAngle = _targetAngle - _thetaCurrent;
// determine the command (proportionnal controller) i.e. angular velocity
double v = 0.0;
double w = _Kp_angle * _errorAngle;
// compute the command
double leftWheelSpeed = v / (_wheelDiameter/2) - _distanceBetweenWheels * w / (_wheelDiameter) ;
double rightWheelSpeed = v / (_wheelDiameter/2) + _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;
// for display
Serial.print("Current_angle:"); Serial.print(_thetaCurrent);Serial.print(",");
Serial.print("Target_angle:"); Serial.println(_targetAngle);
}
void move(int leftMotorSpeed, int rightMotorSpeed)
{
_leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
_rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
}
void stop()
{
_leftMotor.stop();
_rightMotor.stop();
}
/*
SerialEvent occurs whenever a new data comes in the hardware serial RX. This
routine is run between each time loop() runs, so using delay inside loop can
delay response. Multiple bytes of data may be available.
*/
void serialEvent()
{
while (Serial.available())
{
// gets the new byte:
char inChar = (char)Serial.read();
// adds it to the _inputString:
_inputString += inChar;
// if the incoming character is a newline, set a flag so the main loop can
// do something about it:
if (inChar == '\n')
{
_stringComplete = true;
}
}
}

BIN
ultrasonicSensor/.DS_Store vendored
View File

Binary file not shown.

24
ultrasonicSensor/ultrasonicSensor/ultrasonicSensor.ino
View File

@ -1,24 +0,0 @@
#include "Arduino.h"
#include "Wire.h"
#include "MeMCore.h"
// Declare 1 variable of type MeUltrasonicSensor at PORT_3
MeUltrasonicSensor sensor(PORT_3);
void setup() {
// Initialize the serial port at 9600 baud
Serial.begin(9600);
}
void loop() {
// Use the distanceCm() method to retrieve the distance of the obstacle
float distance = sensor.distanceCm( );
// Use the print() method of the Serial port to send the distance
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
// Add a small delay to avoid continuous printing
delay(100);
}