/* Retract Project for RCKolik
by Zafer SAHIN
This example code is in the public domain.
modified 09/01/2015
by Zafer SAHIN
Version 0.22
*/
#include <Servo.h>
Servo lid_servo_left; // Servo object for left retract lid
Servo lid_servo_right; // Servo object for right retract lid
int motor_left[] = {3, 11}; // Arduino pins for controlling left retract motor
int motor_right[] = {5, 6}; // Arduino pins for controlling right retract motor
int lid_servo[] = {9, 10}; // Arduino pins for controlling lid servos
int rxPin = 12; // Arduino pin for receiver pin
int servoPosClose = 180; // Degree type value for positioning the lid servos closed
int servoPosOpen = 0; // Degree type value for positioning the lid servos open
int retractState = 1 ; // Variable to hold retract position, open = 1, stopped = 0, closed = -1
unsigned long previousMillis = 0; // Timer variables
unsigned long retractStopDelay = 4000; // Timer variables
unsigned long currentMillis = 0; // Timer variables
boolean initMillis = true; // Timer variables
unsigned long rxPWMduration; // Variable for reading receiver
void setup() {
pinMode(motor_left[0], OUTPUT);
pinMode(motor_left[1], OUTPUT);
pinMode(motor_right[0], OUTPUT);
pinMode(motor_right[1], OUTPUT);
lid_servo_left.attach(lid_servo[0]);
lid_servo_right.attach(lid_servo[1]);
previousMillis = millis();
}
void loop() {
rxPWMduration = pulseIn(sensePin, HIGH);
if (rxPWMduration > 1600) { // This switch position is for opening the retracts
if (retractState == 1) { // If we are already in retractState = 1, do NOTHING, stop retract motor.
retractStop();
} else { // If we are not in retractState = 1, because we are switched from another state just yet, use a timed action to set state to 1, meanwhile open retracts.
if (initMillis) {
lidMove(servoPosOpen);
previousMillis = millis();
initMillis = false;
}
retractOpen();
currentMillis = millis();
if(currentMillis - previousMillis >= RetractStopDelay) {
retractState = 1;
initMillis = true;
}
}
} else if (rxPWMduration > 1400) { // This switch position is for stopping the retracts
if (retractState == 0) { // If we are already in retractState = 0, do NOTHING, stop retract motor.
retractStop();
} else { // If we are already in retractState = 0, do NOTHING, stop retract motor. Timed action is for future use...
retractStop();
currentMillis = millis();
if(currentMillis - previousMillis >= RetractStopDelay) {
retractState = 0;
initMillis = true;
}
}
} else { // This switch position is for closing the retracts
if (retractState == -1) { // If we are already in retractState = -1, do NOTHING, stop retract motor.
retractStop();
} else { // If we are not in retractState = -1, because we are switched from another state just yet, use a timed action to set state to -1, meanwhile close retracts.
if (initMillis) {
previousMillis = millis();
initMillis = false;
}
retractClose();
currentMillis = millis();
if(currentMillis - previousMillis >= RetractStopDelay) {
retractState = -1;
lidMove(servoPosClose);
initMillis = true;
}
}
}
}
void lidMove(int servoPos){ // Function name says all
lid_servo_left.write(servoPos);
lid_servo_right.write(servoPos);
delay(1000);
}
void retractOpen(){ // Function name says all
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], LOW);
}
void retractClose(){ // Function name says all
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
}
void retractStop(){ // Function name says all
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], LOW);
delay(100);
}