LCD Robot Movements

LCD_Draw_Robot_Steps/LCD_16x2_b2.ino

 #include <LiquidCrystal_I2C.h>
 #include <LiquidCrystal_I2C.h>
+#include <Servo.h>
 
 
 LiquidCrystal_I2C lcd(0x3F, 16, 2); // Address found using lcd scanner, columns, rows
 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 movex = 0;          // Moving and detecting x-coordinates
 //int movey = 0;          // Moving and detecting y-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()
 void setup()
 {
 {
   lcd.begin();
   lcd.begin();
   Serial.begin(9600);
   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! */
   /* Ignore the commented out code! These are for testing purposes! */
 
 
   //  lcd.backlight();
   //  lcd.backlight();
@@ -229,12 +271,12 @@ void changeSegment(int a) {
 //      for(int j=0;j<8;j++)
 //      for(int j=0;j<8;j++)
 //        robotMoving[i][j] = 0b00000;  // OR = 0 OR = 0b0
 //        robotMoving[i][j] = 0b00000;  // OR = 0 OR = 0b0
 //
 //
-//      memset(liigun, 0, sizeof(liigun));
+//    memset(liigun, 0, sizeof(liigun));
 //
 //
-//      //OR
+//    //OR
 //
 //
-//      for (int i=0; i<8; i++)
-//        liigun[i] = B00000; // 0b00000
+//    for (int i=0; i<8; i++)
+//      liigun[i] = B00000; // 0b00000
 //
 //
 //}
 //}
 //
 //