Make: Robotic Arms

183Chapter 9: Inverse Kinematics for 3D Movement

double Zoffset;

double theta;

double alphaOne;

double alphaTwo;

double alphaFinal;

double beta;

void setup() {

servoOne.attach(3);

servoTwo.attach(5);

servoThree.attach(6);

servoFour.attach(9);

servoFive.attach(10);

pinMode(CLK, INPUT);

pinMode(DT, INPUT);

pinMode(SW, INPUT_PULLUP);

lastStateCLK = digitalRead(CLK);

Serial.begin(9600);

}

void loop() {

currentStateCLK = digitalRead(CLK);

int btnState = digitalRead(SW);

if (btnState == LOW ) {

valueSelector++;

delay(250);

}

if(valueSelector == 5){

valueSelector = 1;

}

if (currentStateCLK != lastStateCLK) {

if (digitalRead(DT) == currentStateCLK) ...