While searching the Internet Arduino code that use limit switches to stop servos before they break something and found it at https://gist.github.com/.../8da30ae5d1358c86681d54270f095b9f and I modified the code for my use:
#include <Servo.h>
#define TURN_TIME 1200
Servo toolheadServo;
int toolheadHome = 0;
boolean toolheadGoingHome = false;
int discContact = 0;
boolean toolheadGoingAway = false;
char receivedChar;
boolean newData = false;
//int potpin = A0;
//int val;
void setup()
{
Serial.begin(9600);
pinMode(2, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
pinMode(13, OUTPUT);
toolheadServo.attach(9);
toolheadServo.writeMicroseconds(1500);
Serial.begin(9600);
Serial.println("<Arduino is ready>");
}
void loop()
{
//val = analogRead(potpin);
//val = map(val, 0, 1023, 1000, 2000);
//toolheadServo.writeMicroseconds(val);
//delay(15);
toolheadHome = digitalRead(2);
discContact = digitalRead(3);
Serial.print(toolheadHome);
Serial.println(discContact);
digitalWrite(13, toolheadHome);
digitalWrite(13, discContact);
if (toolheadHome)
{
toolheadServo.writeMicroseconds(1500);
Serial.print("Welcome home, toolhead.");
}
if (discContact)
{
toolheadServo.writeMicroseconds(1500);
Serial.print("Toolhead arrived at destination DVD");
}
recvOneChar();
showNewData();
}
/*
void toolheadGoHome() {
// will default to HIGH with pullup (i.e if switch dies then motor should cut out safety)
if (toolheadHome == 1) {
// good.
// stop!
toolheadServo.detach();
delay(120);
toolheadGoingHome = false;
Serial.print("Welcome home, toolhead.");
} else{
// send it home.
toolheadServo.attach(9);
delay(120);
toolheadServo.write(0);
toolheadGoingHome = true;
Serial.print("Toolhead going home.");
}
}
void toolheadGoExtend() {
// will default to HIGH with pullup (i.e if switch dies then motor should cut out safety)
if (discContact == 0) {
// drive out until disk contact is made
toolheadServo.attach(9);
delay(120);
toolheadServo.write(180);
toolheadGoingAway = true;
Serial.print("Toolhead going away, goodbye");
} else {
// stop when we hit a disc
toolheadServo.detach();
delay(120);
toolheadGoingAway = false;
Serial.print("Welcome to the DVD surface, toolhead.");
}
}
*/
void recvOneChar() {
if (Serial.available() > 0) {
receivedChar = Serial.read();
newData = true;
}
}
void showNewData() {
if (newData == true) {
Serial.println("---");
if (receivedChar == 'A') {
Serial.println("toolheadGoHome");
toolheadServo.writeMicroseconds(1000);
//toolheadGoHome();
}
if (receivedChar == 'B') {
Serial.println("toolheadGoExtend");
toolheadServo.writeMicroseconds(2000);
//toolheadGoExtend();
}
newData = false;
}
}
I didn't install the switches on the robot yet. I thought that limit switches should be on elbow and shoulder joints. Using Serial Monitor to move servos worked but I couldn't get Knob code to work and I commented the code out for now. I have to figure out how to install limit switches on the robot. Any suggestions?