LCD Robot Movements

LCD_Draw_Robot_Steps/LCD_16x2_b2.ino

 #include <LiquidCrystal_I2C.h>
+#include <Servo.h>
 
 LiquidCrystal_I2C lcd(0x3F, 16, 2); // Address found using lcd scanner, columns, rows
 
@@ -39,11 +40,52 @@ int posy = 8;           // Starting y position and the position of y-axis segmen
 //int movex = 0;          // Moving and detecting x-coordinates
 //int movey = 0;          // Moving and detecting y-coordinates
 
+/*
+ * Robot movement stuff!
+ */
+
+/* Define */
+#define rightServo 5   // Define Servo right pin
+#define leftServo  6   // Define Servo left pin
+
+/* Global variables */
+Servo rightWheel;
+Servo leftWheel;
+
+/* Define functions used for the robot to control */
+/* Stop the robot */
+void setWheelsStop() {
+  rightWheel.writeMicroseconds(1500);
+  leftWheel.writeMicroseconds(1500);
+}
+/* Drive forward */
+void setWheelsForward(){
+  rightWheel.write(1300);
+  leftWheel.write(1700);
+}
+/* Drive right */
+void setWheelsRight(){
+  rightWheel.write(1700);
+  leftWheel.write(1700);
+}
+/* Drive left */
+void setWheelsLeft(){
+  rightWheel.write(1300);
+  leftWheel.write(1300);
+}
+
 void setup()
 {
   lcd.begin();
   Serial.begin(9600);
 
+  /* Attach servos to digital pins defined earlier */
+  rightWheel.attach(rightServo);
+  leftWheel.attach(leftServo);
+  
+  /* Put servos to standstill */
+  //setWheelsStop();
+
   /* Ignore the commented out code! These are for testing purposes! */
 
   //  lcd.backlight();