Update 'Deplacement-robot/bluetooth_control/bluetooth_control.ino'

update comments

Deplacement-robot/arrowkeys/arrowkey.py

@@ -0,0 +1,26 @@
+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

Deplacement-robot/arrowkeys/arrowkeys/arrowkeys.ino

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

Deplacement-robot/bluetooth_control/bluetooth_control.ino

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

Deplacement-robot/stepperspeed/stepperspeed.ino

@@ -0,0 +1,440 @@
+//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   
+}
+ 

Position-Robot/extractcoord.py

@@ -8,32 +8,55 @@ import os
 import pygame
 from PIL import Image
 import random
+os.chdir(os.path.dirname(os.path.abspath(__file__)))
+
 
 
 class getpos():
     def __init__(self):
-        self.win=pygame.display.set_mode((1600, 720))
+        # init window
-        self.background = (0,0,0)
+        self.window=pygame.display.set_mode((1600, 720))
-        self.Done = True
+
+        # coord corner:
         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.img = Image.open(('board.jpg'))
+            
-            image = self.img.resize((int(1600), int(720)))
+            self.draw()
-            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]:
@@ -45,7 +68,98 @@ 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)