lucas.marais
/
Club-Robotique
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: lucas.marais:develop
Branches Tags
lucas.marais:main
lucas.marais:develop
..
compare: lucas.marais:main
Branches Tags
lucas.marais:develop
lucas.marais:main

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

6 changed files with 11 additions and 933 deletions
Show Stats Expand all files Collapse all files

26
Deplacement-robot/arrowkeys/arrowkey.py
View File

@ -1,26 +0,0 @@
import serial
import time
import keyboard
ser = serial.Serial('COM16', 38400) # Replace '/dev/ttyACM0' with your Arduino's port
def send_command(command):
ser.write(command.encode())
time.sleep(0.1)
def check_key(key, command):
while True:
if keyboard.is_pressed(key):
send_command(command)
else:
send_command('0')
break
# binding keys to commands
keyboard.add_hotkey('up', check_key, args=('up', '1')) # forward
keyboard.add_hotkey('down', check_key, args=('down', '2')) # backward
keyboard.add_hotkey('left', check_key, args=('left', '3')) # left
keyboard.add_hotkey('right', check_key, args=('right', '4')) # right
keyboard.add_hotkey('space', send_command, args=('0',)) # stop
keyboard.wait('esc') # wait for 'esc' to exit the program

117
Deplacement-robot/arrowkeys/arrowkeys/arrowkeys.ino
View File

@ -1,117 +0,0 @@
#include <SoftwareSerial.h>
#include <AccelStepper.h>
#define RX_PIN 13 // Connected to HC-05 TX pin
#define TX_PIN 12 // Connected to HC-05 RX pin
#define LED_PIN 2 // Connected to LED pin
long receivedSpeed = 800; //Steps / second
char receivedCommand;
int directionMultiplier = 1;
int directionMultiplier2 = -1;
bool runallowed = false;
AccelStepper stepper(1, 8, 9);
AccelStepper stepper2(1, 5, 6);
SoftwareSerial bluetooth(RX_PIN, TX_PIN);
void setup() {
Serial.begin(38400);
bluetooth.begin(38400);
pinMode(LED_PIN, OUTPUT);
stepper.setMaxSpeed(10000);
stepper.setAcceleration(1000);
stepper.disableOutputs();
stepper2.setMaxSpeed(10000);
stepper2.setAcceleration(1000);
stepper2.disableOutputs();
}
void loop() {
readinfo();
RunTheMotor();
}
void RunTheMotor()
{
if (runallowed)
{
stepper.enableOutputs();
stepper.runSpeed(); // use runSpeed instead of run
stepper2.enableOutputs();
stepper2.runSpeed(); // use runSpeed instead of run
}
else
{
stepper.disableOutputs();
stepper.stop(); // stop the stepper
stepper2.disableOutputs();
stepper2.stop(); // stop the stepper
}
}
void RotateRelative()
{
runallowed = true;
stepper.setSpeed(directionMultiplier * receivedSpeed); // set speed and direction
}
void RotateRelative2()
{
runallowed = true;
stepper2.setSpeed(directionMultiplier2 * receivedSpeed); // set speed and direction
}
void readinfo()
{
if (bluetooth.available() > 0)
{
receivedCommand = bluetooth.read();
switch (receivedCommand)
{
case '1': // move forward
directionMultiplier = -1;
directionMultiplier2 = 1;
bluetooth.println("Moving forward.");
RotateRelative();
RotateRelative2();
break;
case '2': // move backward
directionMultiplier = 1;
directionMultiplier2 = -1;
bluetooth.println("Moving backward.");
RotateRelative();
RotateRelative2();
break;
case '3': // turn left
directionMultiplier = 1;
directionMultiplier2 = 1;
bluetooth.println("Turning left.");
RotateRelative();
RotateRelative2();
break;
case '4': // turn right
directionMultiplier = -1;
directionMultiplier2 = -1;
bluetooth.println("Turning right.");
RotateRelative();
RotateRelative2();
break;
case '0': //stop
runallowed = false;
bluetooth.println("Stop.");
break;
default:
break;
}
}
}

225
Deplacement-robot/bluetooth_control/bluetooth_control.ino
View File

@ -1,225 +0,0 @@
#include <SoftwareSerial.h>
#define RX_PIN 13 // Connected to HC-05 TX pin
#define TX_PIN 12 // Connected to HC-05 RX pin
#define LED_PIN 2 // Connected to LED pin
#include <AccelStepper.h>
long receivedSteps = 0; //Number of steps
long receivedSpeed = 0; //Steps / second
long receivedAcceleration = 0; //Steps / second^2
char receivedCommand;
//-------------------------------------------------------------------------------
int directionMultiplier = 1;
int directionMultiplier2 = -1;// = 1: positive direction, = -1: negative direction
bool newData, runallowed = false; // booleans for new data from serial, and runallowed flag
AccelStepper stepper(1, 9, 10);// pulses Digital 8 (CLK) direction Digital 9 (CCW),
AccelStepper stepper2(1, 5, 6);
SoftwareSerial bluetooth(RX_PIN, TX_PIN); // Create a software serial object
void setup() {
Serial.begin(38400); // Initialize the serial monitor
bluetooth.begin(38400); // Initialize the Bluetooth communication
pinMode(LED_PIN, OUTPUT); // Set LED pin as output
stepper.setMaxSpeed(10000); //SPEED = Steps / second
stepper.setAcceleration(800); //ACCELERATION = Steps /(second)^2
stepper.disableOutputs(); //disable outputs
stepper2.setMaxSpeed(10000); //SPEED = Steps / second
stepper2.setAcceleration(800); //ACCELERATION = Steps /(second)^2
stepper2.disableOutputs(); //disable outputs
}
void loop() {
readinfo();
RunTheMotor();
}
void RunTheMotor() //function for the motor
{
if (runallowed == true)
{
stepper.enableOutputs(); //enable pins
stepper.run(); //step the motor (this will step the motor by 1 step at each loop)
stepper2.enableOutputs(); //enable pins
stepper2.run(); //step the motor (this will step the motor by 1 step at each loop)
}
else //program enters this part if the runallowed is FALSE, we do not do anything
{
stepper.disableOutputs(); //disable outputs
stepper2.disableOutputs(); //disable outputs
return;
}
}
void GoHome()
{
if (stepper.currentPosition() == 0)
{
bluetooth.println("We are at the home position.");
stepper.disableOutputs(); //disable power
}
else
{
stepper.setMaxSpeed(800); //set speed manually to 400. In this project 400 is 400 step/sec = 1 rev/sec.
stepper.moveTo(0); //set abolute distance to move
}
}
void RotateRelative()
{
//We move X steps from the current position of the stepper motor in a given direction.
//The direction is determined by the multiplier (+1 or -1)
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper.setMaxSpeed(receivedSpeed); //set speed
stepper.move(directionMultiplier * receivedSteps); //set relative distance and direction
}
void RotateAbsolute()
{
//We move to an absolute position.
//The AccelStepper library keeps track of the position.
//The direction is determined by the multiplier (+1 or -1)
//Why do we need negative numbers? - If you drive a threaded rod and the zero position is in the middle of the rod...
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper.setMaxSpeed(receivedSpeed); //set speed
stepper.moveTo(directionMultiplier * receivedSteps); //set relative distance
}
void GoHome2()
{
if (stepper2.currentPosition() == 0)
{
bluetooth.println("We are at the home position.");
stepper2.disableOutputs(); //disable power
}
else
{
stepper2.setMaxSpeed(800); //set speed manually to 400. In this project 400 is 400 step/sec = 1 rev/sec.
stepper2.moveTo(0); //set abolute distance to move
}
}
void RotateRelative2()
{
//We move X steps from the current position of the stepper motor in a given direction.
//The direction is determined by the multiplier (+1 or -1)
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper2.setMaxSpeed(receivedSpeed); //set speed
stepper2.move(directionMultiplier2 * receivedSteps); //set relative distance and direction
}
void RotateAbsolute2()
{
//We move to an absolute position.
//The AccelStepper library keeps track of the position.
//The direction is determined by the multiplier (+1 or -1)
//Why do we need negative numbers? - If you drive a threaded rod and the zero position is in the middle of the rod...
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper2.setMaxSpeed(receivedSpeed); //set speed
stepper2.moveTo(directionMultiplier2 * receivedSteps); //set relative distance
}
void readinfo()
{
if (bluetooth.available() > 0) //if something comes from the computer
{
receivedCommand = bluetooth.read(); // pass the value to the receivedCommad variable
newData = true; //indicate that there is a new data by setting this bool to true
if (newData == true) //we only enter this long switch-case statement if there is a new command from the computer
{
switch (receivedCommand) //we check what is the command
{
case 'L':
digitalWrite(LED_PIN, HIGH);
break;
case 'O':
digitalWrite(LED_PIN, LOW);
break;
case 'P': //P uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = bluetooth.parseFloat(); //value for the steps
receivedSpeed = bluetooth.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
bluetooth.println("Positive direction."); //print the action
RotateRelative(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case 'N': //N uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = bluetooth.parseFloat(); //value for the steps
receivedSpeed = bluetooth.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
bluetooth.println("Negative direction."); //print action
RotateRelative(); //Run the function
//example: N2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 500 steps/s speed; will rotate in the other direction
//In theory, this movement should take 5 seconds
break;
case '1': //avancer
receivedSteps = bluetooth.parseFloat(); //value for the steps
receivedSpeed = bluetooth.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
directionMultiplier2 = 1;
bluetooth.println("CA AVANCE."); //print the action
RotateRelative();
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case '2': //reculer
receivedSteps = bluetooth.parseFloat(); //value for the steps
receivedSpeed = bluetooth.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
directionMultiplier2 = -1;
bluetooth.println("CA RECULE"); //print the action
RotateRelative();
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case '3': //a gauche
receivedSteps = bluetooth.parseFloat(); //value for the steps
receivedSpeed = bluetooth.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
directionMultiplier2 = 1;
bluetooth.println("CA TOURNE A GAUCHE"); //print the action
RotateRelative();
RotateRelative2(); //Run the function
break;
case '4': //a droite.
receivedSteps = bluetooth.parseFloat(); //value for the steps
receivedSpeed = bluetooth.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
directionMultiplier2 = -1;
bluetooth.println("CA TOURNE A DROITE"); //print the action
RotateRelative();
RotateRelative2(); //Run the function
break;
default:
break;
}
}
}
}

440
Deplacement-robot/stepperspeed/stepperspeed.ino
View File

@ -1,440 +0,0 @@
//Transforming the motor's rotary motion into linear motion by using a threaded rod:
//Threaded rod's pitch = 2 mm. This means that one revolution will move the nut 2 mm.
//Default stepping = 400 step/revolution.
// 400 step = 1 revolution = 8 mm linear motion. (4 start 2 mm pitch screw)
// 1 cm = 10 mm =>> 10/8 * 400 = 4000/8 = 500 steps are needed to move the nut by 1 cm.
//character for commands
/*
'C' : Prints all the commands and their functions.
'P' : Rotates the motor in positive (CW) direction, relative.
'N' : Rotates the motor in negative (CCW) direction, relative.
'R' : Rotates the motor to an absolute positive position (+).
'r' : Rotates the motor to an absolute negative position (-).
'S' : Stops the motor immediately.
'A' : Sets an acceleration value.
'L' : Prints the current position/location of the motor.
'H' : Goes back to 0 position from the current position (homing).
'U' : Updates the position current position and makes it as the new 0 position.
*/
#include <AccelStepper.h>
#include <SoftwareSerial.h>
#define RX_PIN 13 // Connected to HC-05 TX pin
#define TX_PIN 12 // Connected to HC-05 RX pin
SoftwareSerial bluetooth(RX_PIN, TX_PIN); // Create a software serial object
//User-defined values
long receivedSteps = 0; //Number of steps
long receivedSpeed = 0; //Steps / second
long receivedAcceleration = 0; //Steps / second^2
char receivedCommand;
//-------------------------------------------------------------------------------
int directionMultiplier = 1;
int directionMultiplier2 = -1;// = 1: positive direction, = -1: negative direction
bool newData, runallowed = false; // booleans for new data from serial, and runallowed flag
AccelStepper stepper(1, 8, 9);// pulses Digital 8 (CLK) direction Digital 9 (CCW),
AccelStepper stepper2(1, 5, 6);
void setup()
{
Serial.begin(38400); // Initialize the serial monitor
bluetooth.begin(38400); // Initialize the Bluetooth communication
Serial.println("Demonstration of AccelStepper Library"); //print a messages
Serial.println("Send 'C' for printing the commands.");
//setting up some default values for maximum speed and maximum acceleration
Serial.println("Default speed: 400 steps/s, default acceleration: 800 steps/s^2.");
stepper.setMaxSpeed(100000); //SPEED = Steps / second
stepper.setAcceleration(80000); //ACCELERATION = Steps /(second)^2
stepper.disableOutputs(); //disable outputs
stepper2.setMaxSpeed(100000); //SPEED = Steps / second
stepper2.setAcceleration(80000); //ACCELERATION = Steps /(second)^2
stepper2.disableOutputs(); //disable outputs
}
void loop()
{
//Constantly looping through these 2 functions.
//We only use non-blocking commands, so something else (should also be non-blocking) can be done during the movement of the motor
checkSerial();
RunTheMotor(); //function to handle the motor
}
void RunTheMotor() //function for the motor
{
if (runallowed == true)
{
stepper.enableOutputs(); //enable pins
stepper.run(); //step the motor (this will step the motor by 1 step at each loop)
stepper2.enableOutputs(); //enable pins
stepper2.run(); //step the motor (this will step the motor by 1 step at each loop)
}
else //program enters this part if the runallowed is FALSE, we do not do anything
{
stepper.disableOutputs(); //disable outputs
stepper2.disableOutputs(); //disable outputs
return;
}
}
void checkSerial() //function for receiving the commands
{
if (Serial.available() > 0) //if something comes from the computer
{
receivedCommand = Serial.read(); // pass the value to the receivedCommad variable
newData = true; //indicate that there is a new data by setting this bool to true
if (newData == true) //we only enter this long switch-case statement if there is a new command from the computer
{
switch (receivedCommand) //we check what is the command
{
case 'P': //P uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
Serial.println("Positive direction."); //print the action
RotateRelative(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case 'N': //N uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
Serial.println("Negative direction."); //print action
RotateRelative(); //Run the function
//example: N2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 500 steps/s speed; will rotate in the other direction
//In theory, this movement should take 5 seconds
break;
case 'R': //R uses the moveTo() function of the AccelStepper library, which means that it moves absolutely to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
Serial.println("Absolute position (+)."); //print the action
RotateAbsolute(); //Run the function
//example: R800 400 - It moves to the position which is located at +800 steps away from 0.
break;
case 'r': //r uses the moveTo() function of the AccelStepper library, which means that it moves absolutely to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
Serial.println("Absolute position (-)."); //print the action
RotateAbsolute(); //Run the function
//example: r800 400 - It moves to the position which is located at -800 steps away from 0.
break;
case 'S': // Stops the motor
stepper.stop(); //stop motor
stepper.disableOutputs(); //disable power
Serial.println("Stopped."); //print action (with actual setup get a 60° angle)
runallowed = false; //disable running
break;
case 'A': // Updates acceleration
runallowed = false; //we still keep running disabled, since we just update a variable
stepper.disableOutputs(); //disable power
receivedAcceleration = Serial.parseFloat(); //receive the acceleration from serial
stepper.setAcceleration(receivedAcceleration); //update the value of the variable
Serial.print("New acceleration value: "); //confirm update by message
Serial.println(receivedAcceleration); //confirm update by message
break;
case 'L': //L: Location
runallowed = false; //we still keep running disabled
stepper.disableOutputs(); //disable power
Serial.print("Current location of the motor: ");//Print the message
Serial.println(stepper.currentPosition()); //Printing the current position in steps.
break;
case 'H': //H: Homing
runallowed = true;
Serial.println("Homing"); //Print the message
GoHome();// Run the function
break;
case 'U':
runallowed = false; //we still keep running disabled
stepper.disableOutputs(); //disable power
stepper.setCurrentPosition(0); //Reset current position. "new home"
Serial.print("The current position is updated to: "); //Print message
Serial.println(stepper.currentPosition()); //Check position after reset.
break;
case 'C':
PrintCommands(); //Print the commands for controlling the motor
break;
case 'Y': //P uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier2 = -1; //We define the direction
Serial.println("Positive direction."); //print the action
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case 'y': //N uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier2 = 1; //We define the direction
Serial.println("Negative direction."); //print action
RotateRelative2(); //Run the function
//example: N2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 500 steps/s speed; will rotate in the other direction
//In theory, this movement should take 5 seconds
break;
case 'O': //R uses the moveTo() function of the AccelStepper library, which means that it moves absolutely to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier2 = -1; //We define the direction
Serial.println("Absolute position (+)."); //print the action
RotateAbsolute2(); //Run the function
//example: R800 400 - It moves to the position which is located at +800 steps away from 0.
break;
case 'o': //r uses the moveTo() function of the AccelStepper library, which means that it moves absolutely to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier2 = 1; //We define the direction
Serial.println("Absolute position (-)."); //print the action
RotateAbsolute2(); //Run the function
//example: r800 400 - It moves to the position which is located at -800 steps away from 0.
break;
case 'M': // Stops the motor
stepper2.stop(); //stop motor
stepper2.disableOutputs(); //disable power
Serial.println("Stopped."); //print action
runallowed = false; //disable running
break;
case 'K': // Updates acceleration
runallowed = false; //we still keep running disabled, since we just update a variable
stepper2.disableOutputs(); //disable power
receivedAcceleration = Serial.parseFloat(); //receive the acceleration from serial
stepper2.setAcceleration(receivedAcceleration); //update the value of the variable
Serial.print("New acceleration value: "); //confirm update by message
Serial.println(receivedAcceleration); //confirm update by message
break;
case 'G': //L: Location
runallowed = false; //we still keep running disabled
stepper2.disableOutputs(); //disable power
Serial.print("Current location of the motor: ");//Print the message
Serial.println(stepper2.currentPosition()); //Printing the current position in steps.
break;
case 'V': //H: Homing
runallowed = true;
Serial.println("Homing"); //Print the message
GoHome2();// Run the function
break;
case 'E':
runallowed = false; //we still keep running disabled
stepper2.disableOutputs(); //disable power
stepper2.setCurrentPosition(0); //Reset current position. "new home"
Serial.print("The current position is updated to: "); //Print message
Serial.println(stepper2.currentPosition()); //Check position after reset.
break;
case '1': //avancer
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
directionMultiplier2 = 1;
Serial.println("CA AVANCE."); //print the action
RotateRelative();
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case '2': //reculer
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
directionMultiplier2 = -1;
Serial.println("CA RECULE"); //print the action
RotateRelative();
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case '3': //a gauche
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
directionMultiplier2 = 1;
Serial.println("CA TOURNE A GAUCHE"); //print the action
RotateRelative();
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
case '4': //a droite.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
directionMultiplier2 = -1;
Serial.println("CA TOURNE A DROITE"); //print the action
RotateRelative();
RotateRelative2(); //Run the function
//example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
//In theory, this movement should take 5 seconds
break;
default:
break;
}
}
//after we went through the above tasks, newData is set to false again, so we are ready to receive new commands again.
newData = false;
}
}
void GoHome()
{
if (stepper.currentPosition() == 0)
{
Serial.println("We are at the home position.");
stepper.disableOutputs(); //disable power
}
else
{
stepper.setMaxSpeed(800); //set speed manually to 400. In this project 400 is 400 step/sec = 1 rev/sec.
stepper.moveTo(0); //set abolute distance to move
}
}
void RotateRelative()
{
//We move X steps from the current position of the stepper motor in a given direction.
//The direction is determined by the multiplier (+1 or -1)
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper.setMaxSpeed(receivedSpeed); //set speed
stepper.move(directionMultiplier * receivedSteps); //set relative distance and direction
}
void RotateAbsolute()
{
//We move to an absolute position.
//The AccelStepper library keeps track of the position.
//The direction is determined by the multiplier (+1 or -1)
//Why do we need negative numbers? - If you drive a threaded rod and the zero position is in the middle of the rod...
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper.setMaxSpeed(receivedSpeed); //set speed
stepper.moveTo(directionMultiplier * receivedSteps); //set relative distance
}
void PrintCommands()
{
//Printing the commands
Serial.println(" 'C' : Prints all the commands and their functions.");
Serial.println(" 'P' : Rotates the motor in positive (CW) direction, relative.");
Serial.println(" 'N' : Rotates the motor in negative (CCW) direction, relative.");
Serial.println(" 'R' : Rotates the motor to an absolute positive position (+).");
Serial.println(" 'r' : Rotates the motor to an absolute negative position (-).");
Serial.println(" 'S' : Stops the motor immediately.");
Serial.println(" 'A' : Sets an acceleration value.");
Serial.println(" 'L' : Prints the current position/location of the motor.");
Serial.println(" 'H' : Goes back to 0 position from the current position (homing).");
Serial.println(" 'U' : Updates the position current position and makes it as the new 0 position. ");
}
void GoHome2()
{
if (stepper2.currentPosition() == 0)
{
Serial.println("We are at the home position.");
stepper2.disableOutputs(); //disable power
}
else
{
stepper2.setMaxSpeed(800); //set speed manually to 400. In this project 400 is 400 step/sec = 1 rev/sec.
stepper2.moveTo(0); //set abolute distance to move
}
}
void RotateRelative2()
{
//We move X steps from the current position of the stepper motor in a given direction.
//The direction is determined by the multiplier (+1 or -1)
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper2.setMaxSpeed(receivedSpeed); //set speed
stepper2.move(directionMultiplier2 * receivedSteps); //set relative distance and direction
}
void RotateAbsolute2()
{
//We move to an absolute position.
//The AccelStepper library keeps track of the position.
//The direction is determined by the multiplier (+1 or -1)
//Why do we need negative numbers? - If you drive a threaded rod and the zero position is in the middle of the rod...
runallowed = true; //allow running - this allows entering the RunTheMotor() function.
stepper2.setMaxSpeed(receivedSpeed); //set speed
stepper2.moveTo(directionMultiplier2 * receivedSteps); //set relative distance
}

BIN
Position-Robot/board.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 282 KiB

136
Position-Robot/extractcoord.py
View File

@ -8,55 +8,32 @@ import os
import pygame
from PIL import Image
import random
os.chdir(os.path.dirname(os.path.abspath(__file__)))
class getpos():
def __init__(self):
# init window
self.window=pygame.display.set_mode((1600, 720))
# coord corner:
self.win=pygame.display.set_mode((1600, 720))
self.background = (0,0,0)
self.Done = True
self.coords = []
# real table coordinates
self.origin = [[0,0],[2000,0],[3000,2000],[0,3000]]
# pos of robot in wrong coord
self.pos = np.array([])
def main(self):
self.Done = True
while self.Done :
self.draw()
self.img = Image.open(('board.jpg'))
image = self.img.resize((int(1600), int(720)))
size = image.size
mode = image.mode
data = image.tobytes()
self.displayed = pygame.image.fromstring(data, size, mode)
self.rect = self.displayed.get_rect()
self.win.blit(self.displayed, self.rect)
self.checkEvent()
pygame.display.flip()
def draw(self):
# get image
self.img = Image.open(('board.jpg'))
# resise
image = self.img.resize((int(1600), int(720)))
size = image.size
mode = image.mode
data = image.tobytes()
self.displayedSurf = pygame.image.fromstring(data, size, mode)
self.rect = self.displayedSurf.get_rect()
self.window.blit(self.displayedSurf, self.rect)
def checkEvent(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.Done = False
if event.type == pygame.MOUSEBUTTONDOWN:
mouse_presses = pygame.mouse.get_pressed()
if mouse_presses[0]:
@ -68,98 +45,7 @@ class getpos():
print('\n',self.coords,'\n', self.pos,'\n')
X, Y = self.findpos()
print(1, X, Y)
X, Y = self.findpos2()
print(2, X, Y)
print(x,y)
def unit_vector(self, vector):
""" Returns the unit vector of the vector. """
return vector / np.linalg.norm(vector)
def angle_between(self, v1, v2):
v1_u = self.unit_vector(v1)
v2_u = self.unit_vector(v2)
return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))
def findpos(self):
#
# A D
# | E |
# | / \|
# B/______C
#
A = self.coords [0]
B = self.coords [1]
C = self.coords [2]
D = self.coords [3]
E = self.pos
EBC = self.angle_between(C - B, E - B)
ECB = self.angle_between(B - C, E - C)
ABC = self.angle_between(C-B,A-B)
BCD = self.angle_between(D-C,B-C)
# convert angle to real pos
EBC *= np.pi/(ABC*2)
ECB *= np.pi/(BCD*2)
# c = T(ECB)/T(EBC) *2000 / 1+ T(ECB)/T(EBC)
# T(ECB)/T(EBC) = TT
TT = np.tan(ECB) / np.tan(EBC)
X = TT*2000/(1+TT)
Y = np.tan(EBC)*X
return X, Y
def findpos2(self):
# _______
# A D
# | E |
# | / \|
# B/______C
#
A = self.coords [0]
B = self.coords [1]
C = self.coords [2]
D = self.coords [3]
E = self.pos
DAE = self.angle_between(D - A, E - A)
ADE = self.angle_between(E - D, A - D)
BAD = self.angle_between(D-A,B-A)
ADC = self.angle_between(C-D,A-D)
# convert angle to real pos
DAE *= np.pi/(BAD*2)
ADE *= np.pi/(ADC*2)
# c = T(ECB)/T(EBC) *2000 / 1+ T(ECB)/T(EBC)
# T(ECB)/T(EBC) = TT
TT = np.tan(ADE) / np.tan(DAE)
X = TT*2000/(1+TT)
Y = np.tan(ADE)*X
return X, Y
extract = getpos()
extract.main()
X, Y = extract.findpos2()
print(X, Y)
X, Y = extract.findpos()
print(X, Y)