Remove opto endstop logic, ready for hardware testing.

This commit is contained in:
Matt McWilliams 2022-06-28 08:58:31 -04:00
parent c0f056cd8d
commit c3661d6ff1
1 changed files with 12 additions and 43 deletions

View File

@ -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);