Self Leveling Rover

Video showing the rover doing it's self leveling thing. Arduino Uno and a couple of inclinometer sensors take care of the the leveling while I use radio control to steer and propel the rover. Not a great turner but mostly I was interested in the self leveling feature. Someone had mentioned in a comment that the steering could be implemented by using a joint in the middle of the chassis. Good idea. But this project is long gone and have used all the parts in succeeding projects. Wish it was still around to tinker with.

#include <Servo.h>

Servo myservo1,myservo2, myservo3,myservo4;  // create servo object to control a servo

int potpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin
int gyroPin = 3; //tiltmeter VTI SCA610 inclinometer chip (accelerometer)
int gyroPin2 = 5; //tiltmeter VTI SCA610 inclinometer chip (accelerometer)
int fRServo,fLServo,rRServo,rLServo;
int frontTilt;
int sideTilt;
int lowBoy = 1;
///////////////////////////////////////////////////
// this routine controls the speed of the servo 
/////////////////////////////////////////////////
void myServo(int newAngle,int angleInc,int incDelay,int servoNum) {
  int curAngle;
  if (servoNum== 1) {  curAngle = myservo1.read(); }
  if (servoNum== 2) {  curAngle = myservo2.read(); }
  if (servoNum== 3) {  curAngle = myservo3.read(); }
  if (servoNum== 4) {  curAngle = myservo4.read(); }
  if (curAngle < newAngle) {
   for(int angle=curAngle;angle < newAngle;angle += angleInc) {
         if (servoNum == 1) myservo1.write(angle);
         if (servoNum == 2) myservo2.write(angle);
         if (servoNum == 3) myservo3.write(angle);
         if (servoNum == 4) myservo4.write(angle);
         delay(incDelay);   }
   }
   else if (curAngle > newAngle) {
      for(int angle=curAngle;angle > newAngle;angle -= angleInc) {
        if (servoNum == 1) myservo1.write(angle);
        if (servoNum == 2) myservo2.write(angle);
        if (servoNum == 3) myservo3.write(angle);
        if (servoNum == 4) myservo4.write(angle);
        delay(incDelay);   }
   }
   }
int readTilt() {
  int gyroVal  = analogRead(gyroPin);
  gyroVal  = map(gyroVal, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  delay(10);
  Serial.print(" front : ");Serial.println(gyroVal); 
  return(gyroVal);
} // end readTilt()

int readTilt2() { // sideTilt
  int gyroVal2 = analogRead(gyroPin2);
  gyroVal2 = map(gyroVal2, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  delay(10);
  Serial.print(" side : ");Serial.println(gyroVal2);
  return(gyroVal2);
} // end readTilt()
void setup()
{
    Serial.begin(9600);
    delay(100);
    myservo1.attach(3);  // attaches the servo on pin 3 to the servo object
    myservo1.write(90);
    delay(500);                           // waits for the servo to get there 
    myservo2.attach(5);  // attaches the servo on pin 5 to the servo object
    myservo2.write(90);                  // sets the servo position according to the scaled value 
    myservo3.attach(7);  // attaches the servo on pin 5 to the servo object
    myservo3.write(90);                  // sets the servo position according to the scaled value 
    myservo4.attach(9);  // attaches the servo on pin 5 to the servo object
    myservo4.write(90);                  // sets the servo position according to the scaled value 
    delay(500);                           // waits for the servo to get there
    delay(1000);                           // waits for the servo to get there 
    if (lowBoy) {  fRServo = 90; fLServo = 90; rRServo = 90;  rLServo = 90;
      myServo(0,1,5,1);myServo(0,1,5,2);myServo(0,1,5,3);myServo(0,1,5,4); delay(5000);}
    else { fRServo = 10; fLServo = 10; rRServo = 10;  rLServo = 10;}
    //////////////////////////////////////////////////////////////////
    // wheel servo order:       right front is 1
    //                          left  front is 2
    //                          left  rear  is 3
    //                          right rear  is 4
    //////////////////////////////////////////////////////////////////
}

void loop() {
  frontTilt = readTilt();
  sideTilt  = readTilt2();

if (frontTilt > 85 && sideTilt < 90) {
   // front right is lowest point
   if ( rLServo < 110) { // lower opposite wheelheight
    rLServo = rLServo + 2;
    myServo(rLServo,1,5,3);
   }
   if ( rLServo >= 110 && fLServo > 0) { // raise downward wheel
    fRServo = fRServo - 2;
     myServo(fRServo,1,5,1); }
}

 if (frontTilt < 85 && sideTilt > 90) {
   // rear left is lowest point
   //Serial.print(" fRServo : ");Serial.println(fRServo);
   if ( fRServo < 110) { // lower opposite wheel height
    fRServo = fRServo + 2;
    myServo(fRServo,1,5,1);
   }
   if ( rLServo >= 0 && fRServo >= 110) { // raise downward wheel height
    rLServo = rLServo - 2;
     myServo(rLServo,1,5,3); }
}
if (frontTilt > 85 && sideTilt > 90) {
   // front left is lowest point
   if ( rRServo < 110) { // lower opposite wheel
    rRServo = rRServo + 2;
    myServo(rRServo,1,5,4);
   }
   if ( rRServo >= 110 && fRServo > 0) { // raise downward wheel height
    fLServo = fLServo - 2;
     myServo(fLServo,1,5,2); }
}
if (frontTilt < 85 && sideTilt < 90) {
   // rear right is lowest point
   //Serial.print(" fRServo : ");Serial.println(fRServo);
   if ( fLServo < 110) { // lower opposite wheel
    fLServo = fLServo + 2;
    myServo(fLServo,1,5,2);
   }
   if ( rRServo >= 0 && fLServo >= 110) { // raise downward wheel height
    rRServo = rRServo - 2;
     myServo(rRServo,1,5,4); }
}
 //   exit(0); //pause program - hit reset to continue
}