// Skewb-bot Motor Controlling Program
// Eric Boudreau
// This program will take in a string of Skewb notation from a Python program, and will turn all of the motors of the Skewb-bot according to the inputted Skewb notation.
// include the Servo library
#include <Servo.h>
// define pin numbers for Stepper motors
const int dirPin1 = 0;
const int stepPin1 = 1; // stepper 1 for the bottom claw
const int dirPin2 = 2;
const int stepPin2 = 3; // stepper 2 for the left claw (Camera-facing)
const int dirPin3 = 4;
const int stepPin3 = 5; // stepper 3 for the right claw (Camera-facing)
// define constants
int N = 11; // max amount of twists to solve a skewb in given Javascript notation
int x; // Used as a counting variable in for loops.
char incomingString;
// Define servos
Servo servo1; // servo 1 for the bottom claw
Servo servo2; // servo 2 for the left claw
Servo servo3; // servo 3 for the right claw
//******************************************************************************************** Setting Up ****************************************************************************************************//
// main function, runs once
void setup() {
Serial.begin(9600);
// set all pins as outputs
pinMode(dirPin1 ,OUTPUT);
pinMode(stepPin1 ,OUTPUT);
pinMode(dirPin2 ,OUTPUT);
pinMode(stepPin2 ,OUTPUT);
pinMode(dirPin3 ,OUTPUT);
pinMode(stepPin3 ,OUTPUT);
//Define pins for servos and min and max pulse widths based on servo specifications
servo1.attach(6, 771, 2740);
servo2.attach(7, 771, 2740);
servo3.attach(8, 771, 2740);
//define variables.
char notation[N], front[5], back[5], top[5], left[5], right[5], bottom[5], letter;
int k=0;
//************************************************************************************** Reading in the Colors ************************************************************************************************//
// Turn the cube such that the camera can take a picture of each side of the cube. Print a statement to the serial monitor for every image taken.
rotateBottomCounterclockwise(45);
Serial.print("Take Picture of front side\n");
delay(1000);
rotateBottomClockwise(180);
Serial.print("Take Picture of back side\n");
delay(1000);
rotateBottomClockwise(45);
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); //RESET
rotateBottomCounterclockwise(45);
Serial.print("Take Picture of bottom side\n");
delay(1000);
rotateBottomCounterclockwise(180);
Serial.print("Take Picture of top side\n");
delay(1000);
rotateBottomClockwise(45);
gripAndRelease();
flipCubeForward();
ungripAndHold();
flipCubeBackward(); //RESET
rotateBottomCounterclockwise(45);
Serial.print("Take Picture of left side\n");
delay(1000);
rotateBottomClockwise(180);
Serial.print("Take Picture of right side\n");
delay(1000);
// reset cube to initial position
rotateBottomCounterclockwise(135);
gripAndRelease();
flipCubeForward();
ungripAndHold();
flipCubeBackward(); //RESET
//*******************************************************************************************Reading in the String*********************************************************************************************//
notation[0] = 'B';
notation[1] = 'b';
notation[2] = 'U';
notation[3] = 'u';
notation[4] = 'L';
notation[5] = 'l';
notation[6] = 'R';
notation[7] = 'r';
/* Read into Arrays
Send arrays to File
Read File name */
//*********************************************************************************************Solving the Cube************************************************************************************************//
//Run the while loop until the end of the string is reached. Cube will go from unscrambled to solved.
while(k<N)
{
letter = notation[k];
if(letter == 'B'){
Serial.print("Turn back clockwise\n");
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); // RESET
rotateBottomCounterclockwise(90);
reset180Forward(); // RESET
gripAndRelease();
turnRightClockwise();
ungripAndHold();
turnRightCounterclockwise();
rotateBottomClockwise(90);
reset180Backward(); // RESET
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); // RESET
}
if(letter == 'b'){
Serial.print("Turn back counterclockwise\n");
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); // RESET
rotateBottomCounterclockwise(90);
reset180Forward(); // RESET
gripAndRelease();
turnRightCounterclockwise();
ungripAndHold();
turnRightClockwise();
rotateBottomClockwise(90);
reset180Backward(); // RESET
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); //RESET
}
if(letter == 'U'){
Serial.print("Turn top clockwise\n");
rotateBottomCounterclockwise(90);
reset180Forward(); // RESET
gripAndRelease();
turnLeftClockwise();
ungripAndHold();
turnLeftCounterclockwise(); // RESET
rotateBottomClockwise(90);
}
if(letter == 'u'){
Serial.print("Turn top counterclockwise\n");
rotateBottomCounterclockwise(90);
reset180Forward(); // RESET
gripAndRelease();
turnLeftCounterclockwise();
ungripAndHold();
turnLeftClockwise(); // RESET
rotateBottomClockwise(90);
}
if(letter == 'L'){
Serial.print("Turn left clockwise\n");
gripAndRelease();
flipCubeForward();
ungripAndHold();
flipCubeBackward(); // RESET
rotateBottomCounterclockwise(90);
reset180Forward(); // RESET
gripAndRelease();
turnLeftClockwise();
ungripAndHold();
turnLeftCounterclockwise(); // RESET
rotateBottomClockwise(90);
reset180Backward(); // RESET
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); // RESET
}
if(letter == 'l'){
Serial.print("Turn left counterclockwise\n");
gripAndRelease();
flipCubeForward();
ungripAndHold();
flipCubeBackward(); // RESET
rotateBottomCounterclockwise(90);
reset180Forward(); // RESET
gripAndRelease();
turnLeftCounterclockwise();
ungripAndHold();
turnLeftClockwise(); // RESET
rotateBottomClockwise(90);
reset180Backward(); // RESET
gripAndRelease();
flipCubeBackward();
ungripAndHold();
flipCubeForward(); // RESET
}
if(letter == 'R'){
Serial.print("Turn right clockwise\n");
gripAndRelease();
turnRightClockwise();
ungripAndHold();
turnRightCounterclockwise();
}
if(letter == 'r'){
Serial.print("Turn right counterclockwise\n");
gripAndRelease();
turnRightCounterclockwise();
ungripAndHold();
turnRightClockwise();
}
k++;
}
Serial.print("The Skewb is Solved!\n");
return;
}
void loop(){
return;
}
//************************************************************************************Functions used within the main Program***********************************************************************************//
// rotates cube 90 degrees with bottom plate
void rotateBottomClockwise(int angle){
float fraction;
int steps;
digitalWrite(dirPin1, HIGH);
fraction = angle/360;
steps = fraction*200;
for(x=0; x<steps; x++){
digitalWrite(stepPin1, HIGH);
delay(2);
digitalWrite(stepPin1, LOW);
delay(2);
}
delay(500);
}
void rotateBottomCounterclockwise(int angle){
float fraction;
int steps;
digitalWrite(dirPin1, LOW);
fraction = angle/360;
steps = fraction*200;
for(x=0; x<steps; x++){
digitalWrite(stepPin1, HIGH);
delay(2);
digitalWrite(stepPin1, LOW);
delay(2);
}
delay(500);
}
// side arms grip, and bottom arm releases cube.
void gripAndRelease(){
servo2.write(30);
servo3.write(30);
delay(500);
servo2.write(80);
servo3.write(80);
delay(500);
servo1.write(30);
delay(500);
}
// side arms release, and bottom arm grips cube.
void ungripAndHold(){
servo2.write(80);
servo3.write(80);
delay(500);
servo2.write(30);
servo3.write(30);
delay(500);
servo1.write(80);
}
//does a cube 'flip' backward.
void flipCubeBackward(){
digitalWrite(dirPin2, HIGH);
digitalWrite(dirPin3, LOW);
for(x=0; x<67; x++){
digitalWrite(stepPin2, HIGH);
digitalWrite(stepPin3, HIGH);
delay(2);
digitalWrite(stepPin2, LOW);
digitalWrite(stepPin3, LOW);
delay(2);
}
}
//does a cube 'flip' forward.
void flipCubeForward(){
digitalWrite(dirPin2, LOW);
digitalWrite(stepPin3, HIGH);
for(x=0; x<67; x++){
digitalWrite(stepPin2, HIGH);
digitalWrite(stepPin3, HIGH);
delay(2);
digitalWrite(stepPin2, LOW);
digitalWrite(stepPin3, LOW);
delay(2);
}
}
void reset180Backward(){
digitalWrite(dirPin2, HIGH);
digitalWrite(dirPin3, LOW);
for(x=0; x<180; x++){
digitalWrite(stepPin2, HIGH);
digitalWrite(stepPin3, HIGH);
delay(2);
digitalWrite(stepPin2, LOW);
digitalWrite(stepPin3, LOW);
delay(2);
}
}
void reset180Forward(){
digitalWrite(dirPin2, LOW);
digitalWrite(stepPin3, HIGH);
for(x=0; x<180; x++){
digitalWrite(stepPin2, HIGH);
digitalWrite(stepPin3, HIGH);
delay(2);
digitalWrite(stepPin2, LOW);
digitalWrite(stepPin3, LOW);
delay(2);
}
}
// turns the left corner clockwise.
void turnLeftClockwise(){
digitalWrite(dirPin2, LOW);
for(x=0; x<67; x++){
digitalWrite(stepPin2, HIGH);
delay(2);
digitalWrite(stepPin2, LOW);
delay(2);
}
}
// turns the left corner counterclockwise.
void turnLeftCounterclockwise(){
digitalWrite(dirPin2, HIGH);
for(x=0; x<67; x++){
digitalWrite(stepPin2, HIGH);
delay(2);
digitalWrite(stepPin2, LOW);
delay(2);
}
}
// turns the right corner clockwise.
void turnRightClockwise(){
digitalWrite(dirPin3, LOW);
for(x=0; x<67; x++){
digitalWrite(stepPin3, HIGH);
delay(2);
digitalWrite(stepPin3, LOW);
delay(2);
}
}
// turns the right corner counterclockwise.
void turnRightCounterclockwise(){
digitalWrite(dirPin3, HIGH);
for(x=0; x<67; x++){
digitalWrite(stepPin3, HIGH);
delay(2);
digitalWrite(stepPin3, LOW);
delay(2);
}
}
[/SPOILER]
// NOTES:
// The code assumes that the cube is in the correct position initially.