Created
June 11, 2015 15:49
-
-
Save acamilo/8e464aa4e9f38c3aa8bd to your computer and use it in GitHub Desktop.
Code to accept servo signals and controllthe hubmotor
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
void setup() { | |
Serial.begin(115200); | |
#define pin 23 | |
#define pmax 500 | |
#define pmin -500 | |
#define zero 1492 | |
#define INCOM 6 | |
#define EN 8 | |
#define CWCCW 7 | |
#define AIN | |
pinMode(pin, INPUT); // sets the digital pin as output | |
pinMode(13,OUTPUT); | |
pinMode(INCOM,OUTPUT); | |
pinMode(EN,OUTPUT); | |
pinMode(CWCCW,OUTPUT); | |
digitalWrite(INCOM,1); | |
digitalWrite(EN,1); | |
digitalWrite(CWCCW,1); | |
delay(1000); | |
Serial.println("alive"); | |
} | |
int ms=0,timestamp=0; | |
int state = 0; | |
int timeout = 0; | |
#define dbupper 100 | |
#define dblower -100 | |
void loop() { | |
timeout++; | |
if (digitalRead(pin) && state==0){ | |
state = 1; | |
timeout=0; | |
timestamp = micros(); | |
} else if(digitalRead(pin)==0 && state==1) { | |
state = 0; | |
timeout=0; | |
ms = (micros()-timestamp)-1500; | |
Serial.println(ms); | |
} | |
if (ms< dbupper & ms>dblower) digitalWrite(EN,1); | |
else digitalWrite(EN,0); | |
if (ms<dbupper) digitalWrite(CWCCW,1); | |
if (ms>dblower) digitalWrite(CWCCW,0); | |
if (timeout>200000) { | |
Serial.println("Timeout!"); | |
digitalWrite(EN,1); | |
digitalWrite(CWCCW,1); | |
} | |
if (timeout>300000) timeout=200001; | |
digitalWrite(13,digitalRead(pin)); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment