Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// #include <Stepper.h>
#include <Servo.h>
Servo base;
Servo baseopp;
Servo firstjoint;
Servo secondjoint;
Servo gripper;
// Stepper rotate=Stepper(200,8,9,12,13); Commented out because the stepper was not included in final project.
int basepos = 90; //position of the base servo
int firstjointpos=90;
int secondjointpos=90;
int grippos=0;
int reader=0;
// int steps=50;
void setup()
{
//put your setup code here, to run once:
base.attach(7);
baseopp.attach(6);
firstjoint.attach(5);
secondjoint.attach(4);
gripper.attach(3);
Serial.begin(9600);
base.write(basepos);
baseopp.write(basepos); // set servos to 90 b4 we start.
firstjoint.write(firstjointpos);
secondjoint.write(secondjointpos);
gripper.write(grippos);
//rotate.setSpeed(50);
Serial.println("A&S for base, D&F for first joint, G&H for second joint, J&K for gripper");
}
void loop()
{
// put your main code here, to run repeatedly:
reader=Serial.read();
if (reader==83||reader==115) //S
{
Serial.println("The base will move forward");
if (basepos<120) //Restricted to 120 because the servo may start to stall due to the weight.
{
basepos=basepos+30;
base.write(basepos);
baseopp.write(180-basepos);
/* The base needs two servos to run in unison
* This is because the torque of one servo is not high enough to turn the entire metal bar
* baseopp is set to 180-pos because it is facing in the opposite direction. Therefore its angle must be the supplement in order to turn the same way.
*/
}
}
else if (reader==65||reader==97) //A
{
Serial.println("The base will move backward");
if (basepos>60)
{
basepos=basepos-30;
base.write(basepos);
baseopp.write(180-basepos);
}
}
else if (reader==70||reader==102) //D
{
Serial.println("The first joint will move forward");
if (firstjointpos<180)
{
firstjointpos=firstjointpos+30;
firstjoint.write(firstjointpos);
}
}
else if (reader==68||reader==100) //F
{
Serial.println("The first joint will move backward");
if (firstjointpos>0)
{
firstjointpos=firstjointpos-30;
firstjoint.write(firstjointpos);
}
}
else if (reader==71||reader==103) //G
{
Serial.println("The second joint will move forward");
if(secondjointpos<180)
{
secondjointpos=secondjointpos+30;
secondjoint.write(secondjointpos);
}
}
else if (reader==72||reader==104) //H
{
Serial.println("The second joint will move backward");
if (secondjointpos>0)
{
secondjointpos=secondjointpos-30;
secondjoint.write(secondjointpos);
}
}
else if (reader==74||reader==106) //J
{
Serial.println("The gripper will open");
if (grippos<90)
{
grippos=grippos+45;
gripper.write(grippos);
}
}
else if (reader==75||reader==107) //K
{
Serial.println("The gripper will close");
if (grippos>0)
{
grippos=grippos-45;
gripper.write(grippos);
}
}
/* else if (reader==76||reader==108) //L
{
Serial.println("The base will rotate clockwise");
rotate.step(steps);
}
else if (reader==59||reader==58) //; or :
{
Serial.println("The base will rotate counterclockwise");
rotate.step(-steps);
}*/
}
// #include <Stepper.h>
#include <Servo.h>
Servo base;
Servo baseopp;
Servo firstjoint;
Servo secondjoint;
Servo gripper;
// Stepper rotate=Stepper(200,8,9,12,13); Commented out because the stepper was not inluded in final project.
int basepos = 90; //position of the base servo
int firstjointpos=90;
int secondjointpos=90;
int grippos=0;
int reader=0;
// int steps=50;
void setup()
{
//put your setup code here, to run once:
base.attach(7);
baseopp.attach(6);
firstjoint.attach(5);
secondjoint.attach(4);
gripper.attach(3);
Serial.begin(9600);
base.write(basepos);
baseopp.write(basepos); // set servos to 90 b4 we start.
firstjoint.write(firstjointpos);
secondjoint.write(secondjointpos);
gripper.write(grippos);
//rotate.setSpeed(50);
Serial.println("A&S for base, D&F for first joint, G&H for second joint, J&K for gripper");
}
void loop()
{
// put your main code here, to run repeatedly:
reader=Serial.read();
if (reader==83||reader==115) //S
{
Serial.println("The base will move forward");
if (basepos<120) //Restricted to 120 because the servo may start to stall due to the weight.
{
basepos=basepos+30;
base.write(basepos);
baseopp.write(180-basepos);
/* The base needs two servos to run in unison
* This is because the torque of one servo is not high enough to turn the entire metal bar
* baseopp is set to 180-pos because it is facing in the opposite direction. Therefore its angle must be the supplement in order to turn the same way.
*/
}
}
else if (reader==65||reader==97) //A
{
Serial.println("The base will move backward");
if (basepos>60)
{
basepos=basepos-30;
base.write(basepos);
baseopp.write(180-basepos);
}
}
else if (reader==70||reader==102) //D
{
Serial.println("The first joint will move forward");
if (firstjointpos<180)
{
firstjointpos=firstjointpos+30;
firstjoint.write(firstjointpos);
}
}
else if (reader==68||reader==100) //F
{
Serial.println("The first joint will move backward");
if (firstjointpos>0)
{
firstjointpos=firstjointpos-30;
firstjoint.write(firstjointpos);
}
}
else if (reader==71||reader==103) //G
{
Serial.println("The second joint will move forward");
if(secondjointpos<180)
{
secondjointpos=secondjointpos+30;
secondjoint.write(secondjointpos);
}
}
else if (reader==72||reader==104) //H
{
Serial.println("The second joint will move backward");
if (secondjointpos>0)
{
secondjointpos=secondjointpos-30;
secondjoint.write(secondjointpos);
}
}
else if (reader==74||reader==106) //J
{
Serial.println("The gripper will open");
if (grippos<90)
{
grippos=grippos+45;
gripper.write(grippos);
}
}
else if (reader==75||reader==107) //K
{
Serial.println("The gripper will close");
if (grippos>0)
{
grippos=grippos-45;
gripper.write(grippos);
}
}
/* else if (reader==76||reader==108) //L
{
Serial.println("The base will rotate clockwise");
rotate.step(steps);
}
else if (reader==59||reader==58) //; or :
{
Serial.println("The base will rotate counterclockwise");
rotate.step(-steps);
}*/
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.