Skip to content

Commit 13e53c8

Browse files
matiyaumatiyau
committed
Hardware Implementation Beta
1 parent 8f4348f commit 13e53c8

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

60 files changed

+525
-41
lines changed

‎Arduino/cubeTurn-bak/cubeTurn.ino

Lines changed: 232 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,232 @@
1+
//moves=["x", "x2", "x'", "y", "y2", "y'", "U", "U2", "U'", "R", "R2", "R'", "F", "F2", "F'", "RESET"]
2+
3+
#include<Servo.h>
4+
5+
#define SEQ_SRT '*'
6+
#define SEQ_STP '#'
7+
#define SEQ_SEP '<'
8+
9+
#define L_HOLD 11
10+
#define U_HOLD 12
11+
#define R_HOLD 13
12+
#define D_HOLD 7
13+
14+
#define L_ROT 8
15+
#define U_ROT 9
16+
#define R_ROT 10
17+
18+
#define L_GRIP_ANG 150
19+
#define U_GRIP_ANG 150
20+
#define R_GRIP_ANG 150
21+
#define D_GRIP_ANG 150
22+
23+
#define L_UGRIP_ANG 60
24+
#define U_UGRIP_ANG 60
25+
#define R_UGRIP_ANG 60
26+
#define D_UGRIP_ANG 60
27+
28+
#define L_ROT_0 810
29+
#define U_ROT_0 725
30+
#define R_ROT_0 690
31+
32+
#define L_ROT_90
33+
#define U_ROT_90
34+
#define R_ROT_90
35+
36+
#define L_ROT_180 2370
37+
#define U_ROT_180 2445
38+
#define R_ROT_180 2205
39+
40+
41+
Servo servo[7];
42+
43+
char dcsn_char;
44+
byte stepState=0;
45+
byte stepDelay=10;
46+
byte stepPins[4]={2,4,3,5};
47+
48+
byte ROT_PTS[3][3]={{L_ROT_0, L_ROT_90, L_ROT_180}, {U_ROT_0, U_ROT_90, U_ROT_180}, {R_ROT_0, R_ROT_90, R_ROT_180}};
49+
byte ROT_NOW[3]=={1, 1, 1};
50+
51+
void servoWrite(byte servoNum, byte servoPos)
52+
{
53+
servo[servoNum].writeMicroseconds(ROT_PTS[servoNum][servoPos]);
54+
ROT_NOW[servoNum]=servoPos;
55+
}
56+
57+
byte cubeRot(byte moveCode)
58+
{
59+
if (moveCode == 15)
60+
{
61+
servo[0].write(90);
62+
servo[1].write(90);
63+
servo[2].write(90);
64+
servo[3].write(L_UGRIP_ANG);
65+
servo[4].write(U_UGRIP_ANG);
66+
servo[5].write(R_UGRIP_ANG);
67+
servo[6].write(D_GRIP_ANG);
68+
}
69+
else if (moveCode/3 == 0)
70+
{
71+
fullXTurn(moveCode%3);
72+
}
73+
else if (moveCode/3 == 1)
74+
{
75+
fullYTurn(moveCode%3);
76+
}
77+
else if (moveCode/3 == 4)
78+
{
79+
fullYTurn(2);
80+
faceTurn(2, (moveCode%3)+1);
81+
fullYTurn(0);
82+
}
83+
else
84+
{
85+
faceTurn((moveCode/3)-1, (moveCode%3)+1);
86+
}
87+
return 1;
88+
}
89+
90+
void fullXTurn(byte stepCode)
91+
{
92+
grip(2);
93+
if (stepCode==0)
94+
{
95+
servo[0].write(0);
96+
servo[2].write(0);
97+
}
98+
else if (stepCode==2)
99+
{
100+
servo[0].write(180);
101+
servo[2].write(180);
102+
}
103+
ugrip(2);
104+
}
105+
106+
void fullYTurn(byte stepCode)
107+
{
108+
byte stepDir;
109+
if (stepCode==1)
110+
{
111+
stepDir = 1;
112+
stepCode = 100;
113+
}
114+
else
115+
{
116+
stepDir = -(stepCode-1);
117+
stepCode = 50;
118+
}
119+
for (int stepCount = 0; stepCount<stepCode; stepCount++)
120+
{
121+
digitalWrite(stepPins[stepState], LOW);
122+
stepState = (stepState + stepDir + 4) % 4;
123+
digitalWrite(stepPins[stepState], HIGH);
124+
delay(stepDelay);
125+
}
126+
}
127+
128+
void faceTurn(byte faceCode, byte rotCode)
129+
{
130+
byte finPos = (servo[faceCode].read()+90*rotCode)%360;
131+
if (finPos==270)
132+
{
133+
if (rotCode==3)
134+
{
135+
servo[faceCode].write(servo[faceCode].read()+90);
136+
finPos = 0;
137+
}
138+
else
139+
{
140+
servo[faceCode].write(servo[faceCode].read()-90);
141+
finPos = 180;
142+
}
143+
}
144+
grip(faceCode);
145+
servo[faceCode].write(finPos);
146+
delay(500);
147+
ugrip(faceCode);
148+
}
149+
150+
void grip(byte face)
151+
{
152+
if (face==0 || face==2)
153+
{
154+
servo[3].write(L_GRIP_ANG);
155+
servo[5].write(R_GRIP_ANG);
156+
delay(500);
157+
servo[6].write(D_UGRIP_ANG);
158+
delay(500);
159+
}
160+
else
161+
{
162+
servo[4].write(U_GRIP_ANG);
163+
delay(500);
164+
}
165+
}
166+
167+
void ugrip(byte face)
168+
{
169+
if (face==0 || face==2)
170+
{
171+
servo[6].write(D_GRIP_ANG);
172+
delay(500);
173+
servo[3].write(L_UGRIP_ANG);
174+
servo[5].write(R_UGRIP_ANG);
175+
delay(500);
176+
}
177+
else
178+
{
179+
servo[4].write(U_UGRIP_ANG);
180+
delay(500);
181+
}
182+
}
183+
184+
185+
void setup() {
186+
// put your setup code here, to run once:
187+
Serial.begin(115200);
188+
pinMode(2,OUTPUT);
189+
pinMode(3,OUTPUT);
190+
pinMode(5,OUTPUT);
191+
pinMode(5,OUTPUT);
192+
servo[0].attach(L_ROT, L_ROT_PTS[0], L_ROT_PTS[2]);
193+
servo[1].attach(U_ROT, U_ROT_PTS[0], U_ROT_PTS[2]);
194+
servo[2].attach(R_ROT, R_ROT_PTS[0], R_ROT_PTS[2]);
195+
servo[3].attach(L_HOLD, 600, 2150);
196+
servo[4].attach(U_HOLD, 650, 2200);
197+
servo[5].attach(R_HOLD, 535, 2185);
198+
servo[6].attach(D_HOLD, 630, 2340);
199+
servo[0].write(90);
200+
servo[1].write(90);
201+
servo[2].write(90);
202+
servo[3].write(L_UGRIP_ANG);
203+
servo[4].write(U_UGRIP_ANG);
204+
servo[5].write(R_UGRIP_ANG);
205+
servo[6].write(D_GRIP_ANG);
206+
}
207+
208+
void loop() {
209+
// put your main code here, to run repeatedly:
210+
if (Serial.available()<1)
211+
{
212+
return;
213+
}
214+
if (Serial.read()!=SEQ_SRT)
215+
{
216+
return;
217+
}
218+
dcsn_char=Serial.read();
219+
while (dcsn_char!=SEQ_STP)
220+
{
221+
byte mov = Serial.parseInt();
222+
//Serial.println(mov);
223+
if (!cubeRot(mov))
224+
{
225+
Serial.print("Error");
226+
return;
227+
}
228+
229+
dcsn_char=Serial.read();
230+
}
231+
Serial.write('N');
232+
}

0 commit comments

Comments
 (0)