Update: Stair Climbing Code

Summary: Code for climbing up the stairs.

***********************************

#include <Wire.h>

#include <Adafruit_MotorShield.h>

#include "utility/Adafruit_PWMServoDriver.h"

Adafruit_MotorShield AFMSb(0x61);

Adafruit_MotorShield AFMSt(0x60);

//Motors on bottom board:

// Stepper Motor on M1 and M2

Adafruit_StepperMotor *BMotor = AFMSb.getStepper(200, 1);

// Stepper Motor on M3 and M4

Adafruit_StepperMotor *FMotor = AFMSb.getStepper(200, 2);

//Motors on top board:

//DC Motor on M1

Adafruit_DCMotor *LMotor = AFMSt.getMotor(1);

//DC Motor on M2

Adafruit_DCMotor *RMotor = AFMSt.getMotor(2);

//Sets lift motor directions

int up = FORWARD;

int down = BACKWARD;

//Sets motor speeds

int liftSpeed = 50;

int moveSpeed = 50;

int count = 0;

void setup() {

 

  AFMSb.begin();

  AFMSt.begin(); 

}

void loop() {

 

  upStep();

  upStep();

  downStep();

  downStep();

}

//function for moving forward

void moveForward(int time){

 

  LMotor->setSpeed(moveSpeed);

  RMotor->setSpeed(moveSpeed);

 

  LMotor->run(FORWARD);

  RMotor->run(FORWARD);

 

  delay(time);

  LMotor->run(RELEASE);

  RMotor->run(RELEASE);

 }

//function for moving backward

void moveBackward(int time){

 

  LMotor->setSpeed(moveSpeed);

  RMotor->setSpeed(moveSpeed);

 

  LMotor->run(BACKWARD);

  RMotor->run(BACKWARD);

 

  delay(time);  

  LMotor->run(RELEASE);

  RMotor->run(RELEASE);

 }

//function for lifting, length in steps

void liftUp(int length){

 

  while(count <= length){

 

    BMotor->setSpeed(liftSpeed);

    FMotor->setSpeed(liftSpeed);

 

    BMotor->step(1,up,DOUBLE);

    FMotor->step(1,up,DOUBLE);

   

    count++;

  }

  count = 0;

}

//function for lifting, length in steps

void liftDown(int length){

 

  while(count <= length){

 

    BMotor->setSpeed(liftSpeed);

    FMotor->setSpeed(liftSpeed);

 

    BMotor->step(1,down,DOUBLE);

    FMotor->step(1,down,DOUBLE);

   

    count++; 

  }

  count = 0;

}

//function for lifting, time in milliseconds

void liftHold(int time){

 

delay(time);

 

}

void upStep(){

 

  //Move up 

  liftUp(200);

 

  //Tip forward

  BMotor->step(200,up,DOUBLE);

 

  //Retract front legs

  FMotor->step(200,down,DOUBLE);

 

  //Get on step

  moveForward(2000);

 

  //Retract rear legs

  BMotor->step(200,down,DOUBLE); 

}

void downStep(){

 

  //Deploy rear legs

  BMotor->step(200,up,DOUBLE);

 

  //Back up

  moveBackward(2000);

 

  //Deploy front legs

  FMotor->step(200,up,DOUBLE);

 

  //Lower down

  liftDown(200); 

}

***********************************