Code: Select all
#include <Adafruit_MotorShield.h>
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor = AFMS.getMotor (1);
// Global variables to set motor timing
// Each funcion (opening, closing) has a max run time and two phases, before middle limit switch (1) and after (2)
// Each phase has a time that the speed ramps up and a beginnig speed and end speed.
// Times are in milliseconds. Speeds are in PWM value from 0 - 255.
const int openingMaxRunTime = 1000*9;
const int openingRampTime1 = 2500;
const int openingStartSpeed1 = 40;
const int openingEndSpeed1 = 80;
const int openingRampTime2 = 2000;
const int openingStartSpeed2 = 80;
const int openingEndSpeed2 = 255;
const int closingMaxRunTime = 1000*9;
const int closingRampTime1 = 2000;
const int closingStartSpeed1 = 100;
const int closingEndSpeed1 = 255;
const int closingRampTime2 = 1500;
const int closingStartSpeed2 = 150;
const int closingEndSpeed2 = 110;
const int closingTimeAfterLimitSwitch = 1000; //Time after closed limit switch hit before motors are stopped
const int demoOpenDelay = 5000; //5 second delay after opening
const int demoCloseDelay = 5000; //5 second delay after closing
//Set to 2 if controller uses 2 pins to control motor direction (i.e. HIGH,LOW for Forward),
// set to 1 if motor controller uses 1 pin (i.e. HIGH for forward, LOW for reverse)
const int motorControllerPins = 2;
// connect motor controller pins to Arduino digital pins
// motor one and two connected to same pins
//int enA = D0;
//int in1 = D2;
//int in2 = D3;
// limit switches
int lr = A3; //Close/fully retracted limit switch
int ls = A4; //Limit switch used to trigger speed change
int le = A5; //Open/fully extended limit switch
// buttons
int bo = A0; //opening button
int bs = A1; //stop button
int bc = A2; //close button
int bd = D5; //demo button
int allStop = 0; //Global variable to stop all actions
int working = 0; //Global variable in process of performing action
void setup()
{
// set all the motor control pins to outputs
//pinMode(enA, OUTPUT);
//pinMode(in1, OUTPUT);
//pinMode(in2, OUTPUT);
pinMode(lr, INPUT_PULLUP);
pinMode(ls, INPUT_PULLUP);
pinMode(le, INPUT_PULLUP);
pinMode(bo, INPUT_PULLUP);
pinMode(bs, INPUT_PULLUP);
pinMode(bc, INPUT_PULLUP);
//pinMode(bd, INPUT_PULLUP);
//pinMode(setupButton, INPUT_PULLUP);
controlMotors(0,0);
}
void loop() {
allStop = 0;
if(digitalRead(bo)==LOW)
openMount();
if(digitalRead(bc)==LOW)
closeMount();
if(digitalRead(bd)==LOW)
demoButtonPushed();
delay(10);
}
/*void demoButtonPushed() {
while(!allStop){
if(!allStop) openMount();
for(int x=0; x<demoOpenDelay/20 && digitalRead(bs); x++)
delay(20);
if(!allStop) closeMount();
for(int x=0; x<demoCloseDelay/20 && digitalRead(bs); x++)
delay(20);
}
}*/
// this code is not run. it was used when an interrupt routine ran on stop button pushed.
void stopButtonPushed() {
controlMotors(0,0);
allStop = 1;
}
void openMount() {
//Particle.publish("opening");
int startTime = millis();
int startTime2 = 0;
int mySpeed = 0;
while(!allStop && (millis()-startTime)<openingMaxRunTime && digitalRead(le) == HIGH) {
if(digitalRead(ls)==LOW) {
if(millis()-startTime<openingRampTime1)
{
if(openingEndSpeed1>openingStartSpeed1)
mySpeed = openingStartSpeed1+(millis()-startTime)/(openingRampTime1/(openingEndSpeed1-openingStartSpeed1));
else
mySpeed = openingStartSpeed1-(millis()-startTime)/(openingRampTime1/(openingStartSpeed1-openingEndSpeed1));
}
else
mySpeed = openingEndSpeed1;
controlMotors(1, mySpeed);
}
else {
if(startTime2 == 0) startTime2 = millis();
if(millis()-startTime2<openingRampTime2)
{
if(openingEndSpeed2>openingStartSpeed2)
mySpeed = openingStartSpeed2+(millis()-startTime2)/(openingRampTime2/(openingEndSpeed2-openingStartSpeed2));
else
mySpeed = openingStartSpeed2-(millis()-startTime2)/(openingRampTime2/(openingStartSpeed2-openingEndSpeed2));
}
else
mySpeed = openingEndSpeed2;
controlMotors(1, mySpeed);
}
if(digitalRead(bs)==LOW){
allStop = 1;
controlMotors(0,0);
}
delay(10);
}
controlMotors(0,0);
}
void closeMount() {
//Particle.publish("closing");
int startTime = millis();
int startTime2 = 0;
int mySpeed = 0;
while(!allStop && (millis()-startTime)<closingMaxRunTime && digitalRead(lr) == HIGH) {
if(digitalRead(ls)==HIGH) {
if(millis()-startTime<closingRampTime1)
{
if(closingEndSpeed1>closingStartSpeed1)
mySpeed = closingStartSpeed1+(millis()-startTime)/(closingRampTime1/(closingEndSpeed1-closingStartSpeed1));
else
mySpeed = closingStartSpeed1-(millis()-startTime)/(closingRampTime1/(closingStartSpeed1-closingEndSpeed1));
}
else
mySpeed = closingEndSpeed1;
controlMotors(2, mySpeed);
}
else {
if(startTime2 == 0) startTime2 = millis();
if(millis()-startTime2<closingRampTime2)
{
if(closingEndSpeed2>closingStartSpeed2)
mySpeed = closingStartSpeed2+(millis()-startTime2)/(closingRampTime2/(closingEndSpeed2-closingStartSpeed2));
else
mySpeed = closingStartSpeed2-(millis()-startTime2)/(closingRampTime2/(closingStartSpeed2-closingEndSpeed2));
}
else
mySpeed = closingEndSpeed2;
controlMotors(2, mySpeed);
}
if(digitalRead(bs)==LOW){
allStop = 1;
controlMotors(0,0);
}
delay(10);
}
delay(closingTimeAfterLimitSwitch);
controlMotors(0,0);
}
// controlMotors
// Direction 0 = brake, 1 = forward, 2 = reverse
// Speed 0 - 255
void controlMotors(int Direction, int Speed) {
if(Direction == 1){
myMotor->run(FORWARD); //AFMS
//digitalWrite(in1, HIGH);
//digitalWrite(in2, LOW);
}
else if(Direction == 2){
myMotor->run(BACKWARD); //AFMS
//digitalWrite(in1, LOW);
//digitalWrite(in2, HIGH);
}
/* else {
digitalWrite(in1, HIGH);
digitalWrite(in2, HIGH);
}*/
//analogWrite(enA, Speed);
myMotor->setSpeed(speed); //AFMS // I took a couple guesses and changed this
}
