Skip to content

Instantly share code, notes, and snippets.

@acamilo
Created June 11, 2015 15:49
Show Gist options
  • Save acamilo/8e464aa4e9f38c3aa8bd to your computer and use it in GitHub Desktop.
Save acamilo/8e464aa4e9f38c3aa8bd to your computer and use it in GitHub Desktop.
Code to accept servo signals and controllthe hubmotor
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