Second half:
void setServo(int s, int angle) // s is which servo it moves (0 to 15).
{
pwm.setPWM(s, 0, pulseWidth(angle));
currentAngle[s] = angle; // The servo's current angle is updated.
}
void commandServo(int s, int angle, int v)
{
targetAngle[s] = angle;
velocity[s] = v;
updateServo[s] = true;
}
void setGripper(int mode)
{
if (mode == 0)
{
commandServo(gripperServo, 50, 3); // Makes the tool close its grip.
}
else if (mode == 1)
{
commandServo(gripperServo, 105, 3); // Makes the tool open its grip.
}
else if (mode == 2)
{
commandServo(gripperServo, 90, 3); // Makes the tool go to its default mode (semi-closed).
}
}
void swingHammer(bool strike)
{
if (strike)
{
commandServo(hammerServo, 45, 50); // Swings the hammer down.
//hammerHasStruck = true;
//runMain = false;
//hammerMillis = millis(); // Saves the time of strike to make it go up after 500 ms (look in the beginning of loop()).
}
else if (!strike)
{
commandServo(hammerServo, 90, 3); // Pulls the hammer up.
}
c++;
}
void setHolder(int mode)
{
int angle;
if (mode == 0) angle = 50;
else if (mode == 1) angle = 90;
else if (mode == 2) angle = 102;
else if (mode == 3) angle = 105;
commandServo(holderServo, angle, 2);
c++;
}
bool hasElapsed(int period)
{
if (timeNow >= lastTime + period)
{
lastTime = timeNow;
return true;
}
else
{
return false;
}
}
bool hasElapsed2(int period)
{
if (timeNow >= lastTime2 + period)
{
lastTime = timeNow;
lastTime2 = timeNow;
return true;
}
else
{
return false;
}
}
void loop()
{
currentMillis = millis();
timeNow = millis();
if (runMain) Main(); // Runs Main().
else if (runMoveObject) moveObject(piece); // Runs moveObject until it's finished, which is when runMoveObject = false.
/*
if (hammerHasStruck && (currentMillis - hammerMillis >= 500))
{
commandServo(hammerServo, 90, 3); // Automatically pulls the hammer up 500 ms after it has struck.
hammerHasStruck = false;
runMain = true;
lastTime = timeNow;
lastTime2 = timeNow;
c++;
}
*/
if (currentMillis - previousMillis >= interval)
{
previousMillis = currentMillis;
for (int s = 0; s < 16; s++)
{
if (updateServo[s] == true) // Checks to see if servo number s needs to be updated.
{
int newAngle; // The next incremented angle to rotate to (not target).
int rotation; // Which way (up or down) the servo should rotate.
if (targetAngle[s] > currentAngle[s]) rotation = 1; // Sets the rotation direction depending
else rotation = -1; // on whether the target angle is over or below current.
if (currentAngle[s] != targetAngle[s]) // Keep going as long as it hasn't reached its target.
{
newAngle = currentAngle[s] + rotation * (velocity[s]); // Assigns the position of the next step.
if ((rotation == 1 && newAngle > targetAngle[s]) // If the next step exceeds target, rotate to target instead.
|| (rotation == -1 && newAngle < targetAngle[s]))
newAngle = targetAngle[s];
pwm.setPWM(s, 0, pulseWidth(newAngle)); // Rotate servo to the new angle(the next step.)
currentAngle[s] = newAngle; // Update the current angle of rotated servo.
if (currentAngle[s] == targetAngle[s])
{
updateServo[s] = false; // Stop updating and rotating this servo if it has reached its target.
break;
}
}
}
}
}
}
int pulseWidth(int angle)
{
int pulse_wide, analog_value;
pulse_wide = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
return analog_value;
}
//void wait(int period)
//{
// digitalWrite(LED_BUILTIN, HIGH);
// int time_now = millis();
// while (millis() < time_now + period) loop();
// digitalWrite(LED_BUILTIN, LOW);
//
//}
/*
for (int a = 0; a < 11; a++) // Test loop: Makes the servo go from 1 % to 100%, 10% at a time.
{
int b;
if (a == 0) a = 1;
setServo(0, 0); // Set servo number 0 to 0 degrees ASAP.
delay(500);
moveServo(0, 180, a * 10); // Move servo number 0 to 180 degrees at speed a.
delay(2000);
}
*/
/*
void moveServo(int s, int targetAngle, int velocity)
{
int newAngle; // The next incremented angle to rotate to (not target). Should this be double/float?
int rotation; // Which way (up or down) the servo should rotate.
if (velocity >= 100) setServo(s, targetAngle); // Makes sure that only 100% is allowed.
else
{
if (targetAngle[s > currentAngle[s]) rotation = 1; // Sets the rotation direction depending
else rotation = -1; // on whether the target angle is over or below.
while (acurrentAngle[s] != targetAngle) // Keep going as long as it hasn't reached its target.
{
newAngle = currentAngle[s] + rotation * (velocity / 10); // Assigns the position of the next step.
if ((rotation == 1 && newAngle > targetAngle) // If the next step exceeds target, rotate to target instead.
|| (rotation == -1 && newAngle < targetAngle))
newAngle = targetAngle;
pwm.setPWM(s, 0, pulseWidth(newAngle)); // Rotate servo to the new angle(the next step.
delay(20); // The most important delay of the whole sketch.
currentAngle[s] = newAngle; // Update the current angle of rotated servo.
if (currentAngle[s] == targetAngle) break; // If it has reached its target, stop rotating.
}
}
}
*/
// commandServo(0, 170, 1);
// commandServo(4, 20, 2);
//
// wait(2000);
//
// commandServo(0, 20, 3);
// wait(2000);
// commandServo(4, 170, 4);
//
// wait(2000);
//
// commandServo(0, 170, 5);
// commandServo(4, 20, 6);
// wait(2000);
//
// commandServo(0, 170, 1);
// wait(2000);
// commandServo(0, 20, 1);
