Remove opto endstop logic, ready for hardware testing.
This commit is contained in:
parent
c0f056cd8d
commit
c3661d6ff1
|
@ -8,31 +8,27 @@ Servo - Arduino
|
||||||
-
|
-
|
||||||
Red - 5V
|
Red - 5V
|
||||||
Black - GND
|
Black - GND
|
||||||
Yellow - PWM Pin (9 in example)
|
Yellow - PWM Pin (9 in this example)
|
||||||
|
|
||||||
Optical Endstop
|
Using TowerPro SG-5010 +
|
||||||
-
|
TowerPro
|
||||||
Red - 5V
|
as servos for development
|
||||||
Black - GND
|
|
||||||
Yellow - Pin 10
|
|
||||||
----------------------------------------------------
|
----------------------------------------------------
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* ------------------------------------------------
|
/* ------------------------------------------------
|
||||||
* pins
|
* pins
|
||||||
* ------------------------------------------------*/
|
* ------------------------------------------------*/
|
||||||
//Arduino Duemilanove
|
//Arduino Duemilanove +
|
||||||
|
//Arduino Uno
|
||||||
const int PIN_SERVO = 9;
|
const int PIN_SERVO = 9;
|
||||||
const int PIN_ENDSTOP = 10;
|
|
||||||
const int PIN_ENDSTOP_PWR = 11;
|
|
||||||
|
|
||||||
volatile boolean running = false;
|
volatile boolean running = false;
|
||||||
volatile boolean cap_state = false;
|
volatile boolean cap_state = false;
|
||||||
volatile boolean endstop_state = false;
|
|
||||||
|
|
||||||
volatile int angle = 0;
|
volatile int angle = 0;
|
||||||
const int cap_on_angle = 153;
|
const int cap_on_angle = 153; // tune this variable to your servo
|
||||||
const int cap_off_angle = 93;
|
const int cap_off_angle = 93; // -60 degrees apart
|
||||||
volatile long timer = 0;
|
volatile long timer = 0;
|
||||||
volatile int current_angle = 0;
|
volatile int current_angle = 0;
|
||||||
|
|
||||||
|
@ -49,6 +45,7 @@ const int serialDelay = 5;
|
||||||
|
|
||||||
Servo servo;
|
Servo servo;
|
||||||
//SG-5010 speed 0.18s / 60 degree
|
//SG-5010 speed 0.18s / 60 degree
|
||||||
|
//
|
||||||
//converted to milliseconds/angle
|
//converted to milliseconds/angle
|
||||||
const float servoSpeed = 400.0 / 60.0;
|
const float servoSpeed = 400.0 / 60.0;
|
||||||
|
|
||||||
|
@ -105,9 +102,7 @@ void identify () {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Pins_init () {
|
void Pins_init () {
|
||||||
pinMode(PIN_ENDSTOP, INPUT_PULLUP);
|
//
|
||||||
pinMode(PIN_ENDSTOP_PWR, OUTPUT);
|
|
||||||
Endstop_on();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Servo_init () {
|
void Servo_init () {
|
||||||
|
@ -115,22 +110,6 @@ void Servo_init () {
|
||||||
Cap_off(true, true);
|
Cap_off(true, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean Read_endstop () {
|
|
||||||
endstop_state = digitalRead(PIN_ENDSTOP) != LOW;
|
|
||||||
log(endstop_state);
|
|
||||||
return endstop_state;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Endstop_on () {
|
|
||||||
digitalWrite(PIN_ENDSTOP_PWR, HIGH);
|
|
||||||
delay(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Endstop_off () {
|
|
||||||
digitalWrite(PIN_ENDSTOP_PWR, LOW);
|
|
||||||
delay(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Servo_angle (int newAngle) {
|
void Servo_angle (int newAngle) {
|
||||||
servo.write(newAngle);
|
servo.write(newAngle);
|
||||||
delay(Servo_delay(newAngle, angle) + 50);
|
delay(Servo_delay(newAngle, angle) + 50);
|
||||||
|
@ -143,18 +122,13 @@ int Servo_delay (int angleA, int angleB) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Cap_off (boolean suppress, boolean force) {
|
void Cap_off (boolean suppress, boolean force) {
|
||||||
Endstop_on();
|
|
||||||
current_angle = servo.read();
|
current_angle = servo.read();
|
||||||
if ( (cap_state || current_angle != cap_off_angle)) {
|
if (cap_state || current_angle != cap_off_angle) {
|
||||||
Servo_angle(cap_off_angle);
|
Servo_angle(cap_off_angle);
|
||||||
cap_state = false;
|
cap_state = false;
|
||||||
} else {
|
} else {
|
||||||
log("Cap already off");
|
log("Cap already off");
|
||||||
}
|
}
|
||||||
/*while (Read_endstop()) {
|
|
||||||
delay(1);
|
|
||||||
}*/
|
|
||||||
Endstop_off();
|
|
||||||
log("Cap_off()");
|
log("Cap_off()");
|
||||||
if (!suppress) {
|
if (!suppress) {
|
||||||
Serial.println(cmd_cap_off);
|
Serial.println(cmd_cap_off);
|
||||||
|
@ -162,18 +136,13 @@ void Cap_off (boolean suppress, boolean force) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Cap_on (boolean suppress, boolean force) {
|
void Cap_on (boolean suppress, boolean force) {
|
||||||
Endstop_on();
|
|
||||||
current_angle = servo.read();
|
current_angle = servo.read();
|
||||||
if ( (!cap_state || current_angle != cap_on_angle)) {
|
if (!cap_state || current_angle != cap_on_angle) {
|
||||||
Servo_angle(cap_on_angle);
|
Servo_angle(cap_on_angle);
|
||||||
cap_state = true;
|
cap_state = true;
|
||||||
} else {
|
} else {
|
||||||
log("Cap already on");
|
log("Cap already on");
|
||||||
}
|
}
|
||||||
/*while (!Read_endstop()) {
|
|
||||||
delay(1);
|
|
||||||
}*/
|
|
||||||
Endstop_off();
|
|
||||||
log("Cap_on()");
|
log("Cap_on()");
|
||||||
if (!suppress) {
|
if (!suppress) {
|
||||||
Serial.println(cmd_cap_on);
|
Serial.println(cmd_cap_on);
|
||||||
|
|
Loading…
Reference in New Issue