Build Instructions

Collapse

• 1

  Step 1

  #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

  }

View all instructions