Commit 5e209309 authored by André Araújo's avatar André Araújo
Browse files

final release

parent 0aa10085
BSD 2-Clause License
Copyright (c) 2017, Ingeniarius, Ltd
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# robotcraft_bot
# Overview
* [1. RobotCraft Bot v2.0](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#1.-RobotCraft-Bot)
* [2. Features](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#2.-Features)
* [3. Hardware](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#3.-Hardware)
* * [3.1. Robot Explode View](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##3.1-Robot-Explode-View)
* * [3.2. Datasheet](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##3.2-Datasheet)
* * [3.3. Robot Layout](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##3.2-Robot-Layout)
* [4. GetStarted](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#4.-Get-Started)
* * [4.1. How to configure wifi access](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##4.1.-How-to-configure-wifi-access)
* * [4.1.1. Local access by HDMI Cable](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md###4.1.1.-Local-access-by-HDMI-Cable)
* * [4.1.2. Local access by ethernet cable](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md###4.1.2.-Local-access-by-ethernet-cable)
* * [4.2. Remote access by NoMachine](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##4.2.-Remote-access-by-NoMachine)
* * [4.3. Remote access by SSH](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##4.3.-Remote-access-by-SSH)
* [5. Repository Overview](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#5.-Repository-Overview)
* [6. ROS Integration (High-Level)](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#6.-ROS-Integration-(High-Level))
* * [6.1. Published Topics](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##6.1.-Published-Topics)
* * [6.2. Subscribed topics](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##6.2.-Subscribed-topics)
* * [6.3. tf transforms provided](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##6.3.-tf-transforms-provided)
* [7. Ingeniarius Cloud (Snap)](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#7.-Ingeniarius-Cloud-(Snap))
* * [7.1. Account Register, Login and Snapp-Ingeniarius](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##7.1.-Account-Register,-Login-and-Snapp-Ingeniarius)
* * [7.2. Snap Ingeniarius example](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md##7.2.-Snap-Ingeniarius-example)
* [8. Support](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/README.md#8.-Support)
# 1. RobotCraft Bot v2.0
![Ingeniarius Store](documentation/images/ingeniarius_logo.png)
[Ingeniarius Store](https://store.ingeniarius.pt/)
[RobotCraft Bot v2.0](https://store.ingeniarius.pt/) is a limited edition of mobile robots constructed during the Robotics Craftsmanship International Academy (RobotCraft).
This robot is based in the Hercules platform, benefiting from the same Arduino-compatible motor controller that can consistently support the current up to 15A and driving voltage in the range of 6V-20V. Nevertheless, this robot additional includes an Arduino Mega board so that the platform can be easily controlled using a PID controller and the 34 PPR encoders. The Arduino firmware (available as download) also allows to control 4 Smart RGB LEDs WS2812B and read measurements taken from 2 Devantech SRF02 ultrasonic range finder and 1 Sharp GP2Y0A21YK infrared proximity sensor.
[![alt text](https://img.youtube.com/vi/T9cAixLaW-8/0.jpg)](https://youtu.be/T9cAixLaW-8)
# 2. Features
![ROS Kinetic](documentation/images/ros_logo.jpg)
A Raspberry Pi 3 for high-level control, with [Ubuntu MATE 16.04 LTS](https://ubuntu-mate.org/blog/ubuntu-mate-xenial-final-release/) installed do the honours by running the [ROS Kinetic Kame](http://wiki.ros.org/kinetic) distribution (ROS with specific routines included) and interfacing with the Arduino Mega 2560 for low-level control (firmware with PID controller included) board through [rosserial](http://wiki.ros.org/rosserial). All necessary topics are available in the ROS side, making it easy to implement any AI algorithm at will – teleoperation and some few other routines are already included in the high-level ROS layer (available as download).
![Ingeniarius Snap!](documentation/images/snap_logo.png)
You think this is not enough? Well, RobotCraft Bot is the very first robotic platform integrated in our [Snap!-based block programming](https://wiki.scratch.mit.edu/wiki/Snap!_(Programming_Language)) platform! The architecture combines ROS standard with the Snap! visual programming language, targeting students from primary to higher education. The interface runs directly on the browser, benefiting from the power of the internet to program the robot.
# 3. Hardware
The robot includes the following main hardware:
Qt. | Description
--- | ------
1 | 4WD Hercules mobile robotic platform with solid shell made up of aluminium and PLA
2 | DC motors
2 | DC motors with 34 PPR encoders
2 | Devantech SRF02 I2C ultrasonic range finder
2 | Sharp GP2Y0A21YK infrared proximity sensor
4 | Smart RGB LEDs WS2812B with integrated driver chip
1 | Hercules dualmotor driver 15A 6-20V Arduino-compatible controller
1 | Arduino Mega 2560
1 | Raspberry Pi 3
1 | MicroSD card 32GB Class 10
1 | Hacker LiPoBattery 7.4V 2400mAh
## 3.1. Robot Explode View
![Robot Explode View](documentation/images/robotcraft_bot_explode_view.jpg)
## 3.2. Datasheet
![Robot Datasheet1](documentation/images/robotcraft_bot_datasheet_1.png)
![Robot Datasheet2](documentation/images/robotcraft_bot_datasheet_2.png)
[Download pdf version](documentation/robotcraft_bot_datasheet.pdf)
## 3.3. RobotLayout
![Robot Layout](documentation/images/robotcraft_bot_layout.png)
[Download pdf version](documentation/robotcraft_bot_layout.pdf)
# 4. Get Started
## 4.1. How to configure wifi access
To configure the WiFi access and, therefore, allow you to easily develop your algorithms within the ROS environment running in the Raspberry Pi 3 (RPI3), you can adopt any of the following approaches: 1) use the RPI3 as CPU and connect the relevant peripherals, such as monitor, keyboard and mouse; 2) use the ethernet connection.
Regardless of the approach, you will always need to use the following credentials:
***Default access credentials:***
**User:** pi
**Pass:** raspberry
### 4.1.1. Local access with RPI3 as CPU
If you have a display with HDMI port available, probably this might be the easiest way to perform this initial configuration. Simply connect a monitor via HDMI, a keyboard and a mouse to the RPI3. Once the RPI3 is powered it will boot and display the Ubuntu Desktop. Go to the top right corner of the Desktop and scan for the desired wifi network and connect with the necessary credentials.
**Note:** Default RPI3 resolution 1280x800.
### 4.1.2. Local access by ethernet cable
You can connect your PC directly to the RPI3 using an ethernet cable. The RPI3 is configured with the static IP ***192.168.1.1*** defined and, therefore, you need to configure your ethernet connection with an IP within the same network (e.g., ***192.168.1.2*** ).
You can install the software [NoMachine](https://www.nomachine.com/), a free remote desktop aplication available for multi OS. After install NoMachine, simply open the aplication and add a new connection with the IP ***192.168.1.1***, and insert the default user and password previously presented. Finally, you will have access to the RPI3 Ubuntu Desktop. Now you can follow the exact same procedure as before, go to the top right corner of the Desktop, and scan for the desired wifi network.
**Note:** If the connection is unable to be established, please reboot your RPI3.
## 4.2. Remote access by NoMachine
After configuring the wifi network (**Step 4.1.**), you should [download NoMachine](https://www.nomachine.com/), in case you did not followed ***Step 4.1.2***. After installing NoMachine, simple open the aplication, scan for available PC in your network, select the RPI3, and connect. Insert the default user and password previously presented. You will then have full access to the RPI3 Ubuntu Desktop.
![Ubuntu Mate Desktop](documentation/images/ubuntu_mate_desktop.png)
## 4.3. Remote access by SSH
You can access the robot by SSH but NoMachine will make your life easier. Nevertheless, if you still go for SSH, here are the steps to follow.
**If you are using a windows based OS in your PC:**
First [download PuTTY](http://www.putty.org/), then choose the option SSH, input your RPI3 IP, and then simply make the connection and insert your user and password.
**If you are using a linux based OS in your PC:**
First you should check your wifi IP, where your RPI3 is connected, by insert the follwing command in a terminal:
```
ifconfig
```
then run the following command:
```
ssh pi@RASPBETTYPI_IP
```
where *RASPBETTYPI_IP* is your RPI3 IP address, after the connection is succesfull accomplish it will be asked user and password.
# 5. Repository Overview
[RobotCraft_bot Arduino Mega Firmware](firmware/robot_firmware) - Robot firmware with all routines to read sensors, PID controller, ROS routines among others.
[RobotCraft_bot Firmware libraries](firmware/robot_libraries) - All libraries that the main firmware is dependent.
[Hercules Arduino Mega Library](firmware/robot_libraries/hercules_robot_serial_bridge) - Serial bridge interface between the Hercules Dual Motor Controller and the Arduino Mega.
[Arduino SRF02 Sonar tools](tools/srf02_sonar_tools) - Examples tools for SRF02 sonar sensor (i2c scanner, change i2c address,...)
[Adafruit NeoPixel Arduino Library](firmware/robot_libraries/Adafruit_NeoPixel) - Arduino library for controlling single-wire-based LED pixels and strip
[RobotCraft_bot ROS package](ros_package/robotcraft_bot) - ROS package with nodes and launch files to bring up connection between Arduino Mega and ROS on RPI3.
[Rosserial Arduino](https://github.com/ros-drivers/rosserial) - ROS client library for small, embedded devices, such as Arduino. (check http://wiki.ros.org/rosserial).
# 6. ROS Integration (High-Level)
As previous refer in the [datasheet](documentation/robotcraft_bot_datasheet.pdf), the communication between the low-level (Arduino Mega [firmware](firmware/robot_firmware)) and RPI is accomplished by using rosserial (http://wiki.ros.org/rosserial).
If you are not familiarized with ROS, we advise you to explore first the framework by trying the following tuturials: http://wiki.ros.org/ROS/Tutorials
#### Robotcraft Bot Driver
To test this architecture, after you flash your Arduino Mega with firmware (already pre-installed in your robot), in the RPI3 side (ROS), open a terminal and run the following commands:
Launch robotcraft driver:
```
roslaunch robotcraft_bot driver.launch
```
Check all available topics by:
```
rostopic list
```
#### Robotcraft Bot Keyboard Teleoperation
Launch robotcraft driver + Keyboard teleoperation node:
```
roslaunch robotcraft_bot teleop.launch
```
And the use you keyboard to control your robot. More info check [teleop_twist_keyboard](http://wiki.ros.org/teleop_twist_keyboard) ROS node.
**Note:** Check the following tutorials:
[Rosserial examples](http://wiki.ros.org/rosserial/Tutorials)
[RobotCraft Bot ros_package](https://github.com/ingeniarius-ltd/robotcraft_bot/blob/master/ros_package)
## 6.1. Published Topics
All topics are pulish at 5Hz.
* **odom** ([geometry_msgs/Pose2D](http://docs.ros.org/api/geometry_msgs/html/msg/Pose2D.html))
Robot odometry with carteesian pose x (meters), y (meters) and theta (radians).
* **velocities** ([std_msgs/Float32MultiArray](http://docs.ros.org/api/std_msgs/html/msg/Float32MultiArray.html))
Real velocities from robot.
*[linearVelocity (m/s), angularVelocity (rad/s), Wheel_1_angularVelocity (rad/s), Wheel_2_angularVelocity (rad/s)]*
* **distance_sensors** ([std_msgs/Float32MultiArray](http://docs.ros.org/api/std_msgs/html/msg/Float32MultiArray.html))
Values from distance sensors.
*[leftSonar, frontIR, rightSensor] (meters)*
## 6.2. Subscribed topics
* **cmd_vel** ([geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html))
Velocity commands to differentially drive the robot.
* **initial_pose** ([geometry_msgs/Pose2D](http://docs.ros.org/api/geometry_msgs/html/msg/Pose2D.html))
Define initital carteesian pose x (meters), y (meters) and theta (radians).
* **RGB_leds** ([std_msgs/UInt8MultiArray](http://docs.ros.org/jade/api/std_msgs/html/msg/UInt8MultiArray.html))
Define RGB colors from the 4 available LEDs.
*[R1,G1,B1,R2,G2,B2,R3,G3,B3,R4,G4,B4] values between 0-255.*
## 6.3. tf transforms provided
tf is publish at 5Hz.
*odom → base_link*
Transform from odometric origin to robot base.
# 7. Ingeniarius Cloud (Snap)
![Ingeniarius myCloud](documentation/images/ingeniarius_my_cloud.png)
https://edu.ingeniarius.pt/
## 7.1. Account Register, Login and Snapp Ingeniarius
To use Snap and Ingeniarius Cloud programming please register firstly, by providing us with your INGENIARUS KEY, available in your robot.
https://edu.ingeniarius.pt/register
After perform your register, please Login in Ingeniarius Cloud.
![Ingeniarius Cloud](documentation/images/ingeniarius_cloud_login.png)
https://edu.ingeniarius.pt/login
### 7.3. Snap!-Ingeniarius example
Once you are logged in the Ingeniarius Cloud in your PC side, open the application ***RobotCraft*** and go to Open > Cloud > Robotcrat > Apply
Then, in the RPI3 side, launch the following command in a terminal:
```
roslaunch robotcraft_bot authentication_server.launch
```
And thats it. enjoy!
![Ingeniarius Cloud](documentation/images/robotcraft_bot_cloud_snap.png)
# 8. Support
For further information please visit ingeniarius (http://ingeniarius.pt/) or contact by email welcome@ingeniarius.pt
/* ROS Libs */
#include <ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/UInt8MultiArray.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
/* Motor Driver Serial Bridge lib */
#include "robot_serial_bridge.h"
/* General use lib */
#include <math.h>
/* I2C Sonars*/
#include <Wire.h>
/* Smart LED lib */
#include "Adafruit_NeoPixel.h"
#ifdef __AVR__
#include <avr/power.h>
#endif
/* Auxiliary function to normalize angle to the -pi, pi domain */
#define NORMALIZE(z) atan2(sin(z), cos(z))
/* General defines */
#define SMARTLED_PIN 7 // SmartLED Arduino PIN
#define SMARTLEDS_NUMBER 4 // Total of LEDs in the array
#define IR_SENSOR_PIN A0 // Infrared proximity sensor PIN
#define SONAR_SENSOR_LEFT_ADD 113 // I2C address of left sonar
#define SONAR_SENSOR_RIGHT_ADD 112 // I2C address of right sonar
#define WHEEL_RADIUS 0.040 // in meters
#define AXIS_LENGTH 0.170 // in meters
#define PULSES_REV 34.0
#define NORMALIZE(z) atan2(sin(z), cos(z)) // auxiliary function to normalize angle to the -pi, pi domain
// Init SmartLed Class
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(SMARTLEDS_NUMBER, SMARTLED_PIN, NEO_GRB + NEO_KHZ800);
// Init Serial communication class
// Communication between motor driver and Arduino Mega use Serial1 TX1 e RX1 (Pins 18 and 19)
robot_serial_bridge robot = robot_serial_bridge(&Serial1);
// Global struct with robot data
struct {
float robot_pose[3] = {0.0}; // x, y, theta
float robot_vel[4] = {0.0}; // linearVel, angularVel, leftAngularWheelVel, rightAngularWheelVel
float Vx_desired_ = 0.0;
float Vw_desired_ = 0.0;
} robot_data;
// PID declarations
const float Kp = 0.7, Kd = 0.01, Ki = 0.4; // PID gains
float Kp_errorR = 0, Kp_errorR_ = 0, Kd_errorR = 0, Ki_errorR = 0;
float Kp_errorL = 0, Kp_errorL_ = 0, Kd_errorL = 0, Ki_errorL = 0;
// Flags and timers
bool ONCE = true;
unsigned long ros_pub_elapsed_time = 0;
unsigned long pid_elapsed_time = 0;
unsigned long cmd_vel_timeout = 0;
// ROS declarations
ros::NodeHandle nh;
geometry_msgs::Pose2D odometry;
geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;
std_msgs::Float32MultiArray velocities;
std_msgs::Float32MultiArray distance_sensors;
ros::Publisher odometryPublisher("odom", &odometry);
ros::Publisher velocitesPublisher("velocities", &velocities);
ros::Publisher sensorsPublisher("distance_sensors", &distance_sensors);
void cmd_velCallback(const geometry_msgs::Twist &msg) {
robot_data.Vx_desired_ = msg.linear.x;
robot_data.Vw_desired_ = msg.angular.z;
cmd_vel_timeout = millis();
}
void initialPoseCallback(const geometry_msgs::Pose2D &msg) {
robot_data.robot_pose[0] = msg.x;
robot_data.robot_pose[1] = msg.y;
robot_data.robot_pose[2] = msg.theta;
}
void ledsCallback(const std_msgs::UInt8MultiArray &msg) {
pixels.setPixelColor(0, pixels.Color(msg.data[0], msg.data[1], msg.data[2]));
pixels.setPixelColor(1, pixels.Color(msg.data[3], msg.data[4], msg.data[5]));
pixels.setPixelColor(2, pixels.Color(msg.data[6], msg.data[7], msg.data[8]));
pixels.setPixelColor(3, pixels.Color(msg.data[9], msg.data[10], msg.data[11]));
pixels.show();
}
ros::Subscriber<geometry_msgs::Twist> cmd_velSubscriber("cmd_vel", &cmd_velCallback);
ros::Subscriber<geometry_msgs::Pose2D> initialPoseSubscriber("initial_pose", &initialPoseCallback);
ros::Subscriber<std_msgs::UInt8MultiArray> ledsSubscriber("RGB_leds", &ledsCallback);
/************************************************************************
Function: leds_off()
Objective:
Issues:
*************************************************************************/
void leds_off() {
pixels.setPixelColor(0, pixels.Color(0, 0, 0));
pixels.setPixelColor(1, pixels.Color(0, 0, 0));
pixels.setPixelColor(2, pixels.Color(0, 0, 0));
pixels.setPixelColor(3, pixels.Color(0, 0, 0));
pixels.show();
}
/************************************************************************
Function: leds_test()
Objective:
Issues:
*************************************************************************/
void leds_test() {
for (int i = 0; i < 2; i++) {
pixels.setPixelColor(0, pixels.Color(255, 0, 0));
pixels.setPixelColor(1, pixels.Color(0, 255, 0));
pixels.setPixelColor(2, pixels.Color(0, 0, 255));
pixels.setPixelColor(3, pixels.Color(0, 255, 0));
pixels.show();
delay(500);
leds_off();
delay(500);
}
}
/************************************************************************
Function: requestUltrasonics()
Objective: Request distance values single I2C sensors
Inputs parameters: I2C sensor address
Outputs parameters:
Return:
Notes:
*************************************************************************/
void requestUltrasonic(int address) {
Wire.beginTransmission(address);
Wire.write(byte(0x00));
Wire.write(byte(0x51));
Wire.endTransmission();
}
/************************************************************************
Function: readUltrasonic()
Objective: Read ultrasonic I2C sensor
Inputs parameters: I2C sensor address
Outputs parameters:
Return:
Notes:
*************************************************************************/
int readUltrasonic(int address) {
int reading = 0;
Wire.beginTransmission(address);
Wire.write(byte(0x02));
Wire.endTransmission();
Wire.requestFrom(address, 2);
if (2 <= Wire.available()) {
reading = Wire.read();
reading = reading << 8;
reading |= Wire.read();
}
return reading;
}
/************************************************************************
Function: requestUltrasonics()
Objective: Request distance values from both I2C sensors
Inputs parameters:
Outputs parameters:
Return:
Notes:
*************************************************************************/
void requestUltrasonics() {
requestUltrasonic(SONAR_SENSOR_RIGHT_ADD);
requestUltrasonic(SONAR_SENSOR_LEFT_ADD);
}
/************************************************************************
Function: readLeft()
Objective: Read left I2C distance sensor
Inputs parameters:
Outputs parameters:
Return:
Notes:
*************************************************************************/
float s_value_L = 0.0;
float readLeft_sonar() {
float s_value = readUltrasonic(SONAR_SENSOR_LEFT_ADD) / 100.0;
if (s_value == 0.0 || s_value > 2.50) {
s_value = s_value_L;
}
s_value_L = s_value;
return s_value;
}
/************************************************************************
Function: readRight()
Objective: Read right I2C distance sensor
Inputs parameters:
Outputs parameters:
Return:
Notes:
*************************************************************************/
float s_value_R = 0.0;
float readRight_sonar() {
float s_value = readUltrasonic(SONAR_SENSOR_RIGHT_ADD) / 100.0;
if (s_value == 0.0 || s_value > 2.50) {
s_value = s_value_R;
}
s_value_R = s_value;
return s_value;
}
/************************************************************************
Function: readFront()
Objective: Read IR distance sensor
Inputs parameters:
Outputs parameters:
Return:
Notes:
*************************************************************************/
float readFront() {
float reading = (float)(29.988 * pow((analogRead(IR_SENSOR_PIN) * 0.0048828125), -1.173)) / 100.0;
if (reading > 0.8)
reading = 0.8;
return reading;
}
/************************************************************************
Function: stop_motors_pid()
Objective:
Issues:
*************************************************************************/
void stop_motors_reset_pid() {
robot_data.Vx_desired_ = 0.0;
robot_data.Vw_desired_ = 0.0;
Kp_errorR = 0.0;
Kp_errorR_ = 0.0;
Kd_errorR = 0.0;
Ki_errorR = 0.0;
Kp_errorL = 0.0;
Kp_errorL_ = 0,0;
Kd_errorL = 0.0;
Ki_errorL = 0.0;
robot.stop_motors();