Skip to content

Commit 0b2e254

Browse files
madhephaestusmadhephaestus
committed
2 parents 8e03227 + f0a12f0 commit 0b2e254

3 files changed

Lines changed: 262 additions & 46 deletions

File tree

‎ArduinoClassRobot.ino‎

Lines changed: 121 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,47 +1,135 @@
11
/*
2-
Blink
2+
Blink
33
4-
Turns an LED on for one second, then off for one second, repeatedly.
4+
Turns an LED on for one second, then off for one second, repeatedly.
55
6-
Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO
7-
it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to
8-
the correct LED pin independent of which board is used.
9-
If you want to know what pin the on-board LED is connected to on your Arduino
10-
model, check the Technical Specs of your board at:
11-
https://www.arduino.cc/en/Main/Products
6+
Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO
7+
it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to
8+
the correct LED pin independent of which board is used.
9+
If you want to know what pin the on-board LED is connected to on your Arduino
10+
model, check the Technical Specs of your board at:
11+
https://www.arduino.cc/en/Main/Products
1212
13-
modified 8 May 2014
14-
by Scott Fitzgerald
15-
modified 2 Sep 2016
16-
by Arturo Guadalupi
17-
modified 8 Sep 2016
18-
by Colby Newman
13+
modified 8 May 2014
14+
by Scott Fitzgerald
15+
modified 2 Sep 2016
16+
by Arturo Guadalupi
17+
modified 8 Sep 2016
18+
by Colby Newman
1919
20-
This example code is in the public domain.
20+
This example code is in the public domain.
2121
22-
https://www.arduino.cc/en/Tutorial/BuiltInExamples/Blink
23-
*/
22+
https://www.arduino.cc/en/Tutorial/BuiltInExamples/Blink
23+
*/
2424
#include <ESP32Servo.h>
25-
Servo left;
26-
Servo right;
25+
#include <WiiChuck.h>
26+
#include <Wire.h>
27+
#include <EasyBNO055_ESP.h>
28+
29+
class Chassis {
30+
public:
31+
int lCenter = 86;
32+
int rCenter = 87;
33+
Servo left;
34+
Servo right;
35+
double fwdTarget = 0;
36+
double rotZTarget = 0;
37+
double currentRotationZ = 0;
38+
double rotZIncrement = 1.2;
39+
double kp = 0.013;
40+
double fwdConstant = 15;
41+
double resetTarget = 0;
42+
int lval;
43+
int rval;
44+
Chassis() {
45+
}
46+
void setTargets(double fwd, double rotz, double currentRotZ) {
47+
if (abs(rotz) < 0.01) {
48+
rotz = 0;
49+
}
50+
fwdTarget = fwd;
51+
rotZTarget += (rotz * rotZIncrement);
52+
currentRotationZ = currentRotZ;
53+
54+
write();
55+
}
56+
void begin() {
57+
left.attach(33, 1000, 2000);
58+
right.attach(32, 1000, 2000);
59+
left.write(lCenter);
60+
right.write(rCenter);
61+
}
62+
void write() {
63+
double rotZErr = -kp * (rotZTarget - currentRotationZ);
64+
lval = fwdConstant * fwdTarget - 90 * rotZErr + lCenter;
65+
rval = -fwdConstant * fwdTarget - 90 * rotZErr + rCenter;
66+
if (lval < 0)
67+
lval = 0;
68+
if (rval < 0)
69+
rval = 0;
70+
if (lval > 180)
71+
lval = 180;
72+
if (rval > 180)
73+
rval = 180;
74+
left.write(lval);
75+
right.write(rval);
76+
}
77+
};
78+
79+
Accessory nunchuck;
80+
EasyBNO055_ESP bno;
81+
Chassis puppy;
82+
83+
void otherI2CUpdate() {
84+
nunchuck.readData(); // Read inputs and update maps
85+
}
86+
87+
float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
88+
const float run = in_max - in_min;
89+
if (run == 0) {
90+
log_e("map(): Invalid input range, min == max");
91+
return -1; // AVR returns -1, SAM returns 0
92+
}
93+
const float rise = out_max - out_min;
94+
const float delta = x - in_min;
95+
return (delta * rise) / run + out_min;
96+
}
97+
2798
// the setup function runs once when you press reset or power the board
2899
void setup() {
29-
left.attach(33);
30-
right.attach(32);
31-
left.write(90);
32-
right.write(90);
33-
100+
101+
Serial.begin(115200);
102+
Serial.println("Starting ESP32");
103+
puppy.begin();
104+
nunchuck.begin();
105+
bno.start(&otherI2CUpdate);
106+
34107
}
35108

36109
// the loop function runs over and over again forever
37110
void loop() {
38-
left.write(0);
39-
right.write(0);
40-
delay(1000); // wait for a second
41-
left.write(180);
42-
right.write(180);
43-
delay(1000); // wait for a second
44-
left.write(90);
45-
right.write(90);
46-
delay(1000);
111+
112+
float x = -fmap(nunchuck.values[1], 0, 255, -1.0, 1.0);
113+
float y = fmap(nunchuck.values[0], 0, 255, -1.0, 1.0);
114+
puppy.setTargets(x, y, bno.orientationZ);
115+
if (nunchuck.values[11] > 0) {
116+
puppy.rotZTarget = puppy.resetTarget;
117+
}
118+
if (nunchuck.values[10] > 0) {
119+
puppy.resetTarget = puppy.currentRotationZ;
120+
puppy.rotZTarget = puppy.currentRotationZ;
121+
}
122+
delay(10);
123+
124+
Serial.print("\n\tx= ");
125+
Serial.print(bno.orientationX);
126+
Serial.print(" |\ty= ");
127+
Serial.print(bno.orientationY);
128+
Serial.print(" |\tz= ");
129+
Serial.print(bno.orientationZ);
130+
Serial.print(" |\tL= ");
131+
Serial.print(puppy.lval);
132+
Serial.print(" |\tR= ");
133+
Serial.print(puppy.rval);
134+
47135
}

‎BaseBot.groovy‎

Lines changed: 109 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -422,12 +422,45 @@ try {
422422
CSG top = parts[1];
423423
CSG bot = parts[0];
424424

425-
425+
double legMountRadius = 10
426+
double legMountPinLength = 10
427+
double legPinRadius = 4
426428
LengthParameter tailLength = new LengthParameter("Cable Cut Out Length",30,[500, 0.01])
427429
tailLength.setMM(25)
430+
428431
CSG motor = Vitamins.get("hobbyServo", "fs90r")
429432
CSG horn = Vitamins.get("hobbyServoHorn", "fs90r_1")
430433
.movez(motor.getMaxZ())
434+
CSG boltWheel = Vitamins.get("chamferedScrew", "M3x16")
435+
.roty(180)
436+
.toZMin()
437+
.movez(motor.getMaxZ())
438+
.movex(legMountRadius)
439+
CSG bearingWheel = Vitamins.get("ballBearing", "695zz")
440+
.movez(motor.getMaxZ()+legMountPinLength)
441+
.movex(legMountRadius)
442+
double legMountPinRadius = bearingWheel.getTotalX()/2+3
443+
444+
CSG legPin = new Cylinder(legPinRadius, legMountPinLength+1).toCSG()
445+
.movez(motor.getMaxZ())
446+
.movex(legMountRadius)
447+
CSG legSquare = new Cube(legMountPinRadius+2,legMountPinRadius*2,bearingWheel.getTotalZ()+1).toCSG()
448+
.toZMin()
449+
.toXMin()
450+
CSG legHole = new Cylinder(3.5/2.0, legMountPinRadius*2).toCSG()
451+
.movez(-legMountPinRadius)
452+
.rotx(90)
453+
.movex(legMountPinRadius-1)
454+
.movez((bearingWheel.getTotalZ()+1)/2.0)
455+
CSG legMount = new Cylinder(legMountPinRadius, bearingWheel.getTotalZ()+1).toCSG()
456+
.union(legSquare)
457+
.difference(legHole)
458+
.difference(new Cylinder(legPinRadius+1, legMountPinLength+1).toCSG())
459+
.movez(motor.getMaxZ()+legMountPinLength-1)
460+
.movex(legMountRadius)
461+
.difference(bearingWheel.hull())
462+
463+
431464
double hornDepth = horn.getTotalZ()
432465
double halfServoDistance = motor.getTotalX()/2
433466
double servoSplit=2
@@ -469,19 +502,57 @@ try {
469502

470503
CSG rightDrive = asmOfDrive.transformed(rightSide)
471504
CSG leftDriveHorn = horn.transformed(leftSide)
472-
505+
CSG leftDriveBolt = boltWheel.transformed(leftSide)
506+
CSG rightDriveBolt = boltWheel.transformed(rightSide)
507+
508+
CSG leftDriveBearing = bearingWheel.transformed(leftSide)
509+
CSG rightDriveBearing = bearingWheel.transformed(rightSide)
510+
511+
512+
CSG leftlegMount = legMount.transformed(leftSide)
513+
CSG rightlegMount = legMount.transformed(rightSide)
514+
473515
CSG rightDriveHorn = horn.transformed(rightSide)
474-
516+
517+
518+
CSG rightLegPin= legPin.transformed(rightSide)
519+
CSG leftLegPin= legPin.transformed(leftSide)
520+
521+
475522
CSG bothDrive = leftDrive.union(rightDrive)
476523

477524

478525

479-
CSG leftWheel = wheepCore.transformed(leftSide).difference(leftDriveHorn)
526+
CSG leftWheel = wheepCore.transformed(leftSide)
527+
.difference(leftDriveHorn)
528+
.union(leftLegPin)
529+
.difference(leftDriveBolt)
530+
.difference(leftDriveBearing)
480531
CSG rightWheel = wheepCore.transformed(rightSide).difference(rightDriveHorn)
532+
.union(rightLegPin)
533+
.difference(rightDriveBolt)
534+
.difference(rightDriveBearing)
535+
536+
leftDriveBearing.setColor(Color.SILVER)
537+
.setManufacturing({ toMfg ->
538+
return null
539+
})
540+
rightDriveBearing.setColor(Color.SILVER)
541+
.setManufacturing({ toMfg ->
542+
return null
543+
})
481544
leftDriveHorn.setColor(Color.BLACK)
482545
.setManufacturing({ toMfg ->
483546
return null
484547
})
548+
leftDriveBolt.setColor(Color.BLACK)
549+
.setManufacturing({ toMfg ->
550+
return null
551+
})
552+
rightDriveBolt.setColor(Color.BLACK)
553+
.setManufacturing({ toMfg ->
554+
return null
555+
})
485556
rightDriveHorn.setColor(Color.BLACK)
486557
.setManufacturing({ toMfg ->
487558
return null
@@ -679,7 +750,14 @@ try {
679750
.setManufacturing({ toMfg ->
680751
return toMfg.roty(-90).toZMin()
681752
})
682-
753+
leftlegMount.setName("leftlegMount")
754+
.setManufacturing({ toMfg ->
755+
return toMfg.roty(-90).toZMin()
756+
})
757+
rightlegMount.setName("rightLegMount")
758+
.setManufacturing({ toMfg ->
759+
return toMfg.roty(90).toZMin()
760+
})
683761
rightWheel.setName("rightWheel")
684762
.setManufacturing({ toMfg ->
685763
return toMfg.roty(90).toZMin()
@@ -740,7 +818,25 @@ try {
740818
tireMovedL.addAssemblyStep(9, new Transform().movex(-30))
741819
tireMovedL.addAssemblyStep(8, new Transform().movex(-10))
742820
leftDriveHorn.addAssemblyStep(10, new Transform().movex(-40))
743-
821+
822+
leftDriveBolt.addAssemblyStep(9, new Transform().movex(-30))
823+
leftDriveBolt.addAssemblyStep(8, new Transform().movex(20))
824+
825+
rightDriveBolt.addAssemblyStep(9, new Transform().movex(30))
826+
rightDriveBolt.addAssemblyStep(8, new Transform().movex(-20))
827+
828+
leftDriveBearing.addAssemblyStep(9, new Transform().movex(-30))
829+
rightDriveBearing.addAssemblyStep(9, new Transform().movex(30))
830+
831+
leftlegMount.addAssemblyStep(9, new Transform().movex(-30))
832+
rightlegMount.addAssemblyStep(9, new Transform().movex(30))
833+
834+
leftlegMount.addAssemblyStep(8, new Transform().movex(-5))
835+
rightlegMount.addAssemblyStep(8, new Transform().movex(5))
836+
837+
leftDriveBearing.addAssemblyStep(8, new Transform().movex(-20))
838+
rightDriveBearing.addAssemblyStep(8, new Transform().movex(20))
839+
744840
rightWheel.addAssemblyStep(9, new Transform().movex(30))
745841
tireMovedR.addAssemblyStep(9, new Transform().movex(30))
746842
tireMovedR.addAssemblyStep(8, new Transform().movex(10))
@@ -765,7 +861,13 @@ try {
765861
hingingPlate,
766862
hingeThread,
767863
closureThreads,
768-
bottomThreads
864+
bottomThreads,
865+
leftDriveBolt,
866+
rightDriveBolt,
867+
leftDriveBearing,
868+
rightDriveBearing,
869+
leftlegMount,
870+
rightlegMount
769871

770872
]
771873
}catch(Throwable tr) {

‎printbed.json‎

Lines changed: 32 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,31 @@
11
{
22
"locations": {
3+
"rightLegMount": {
4+
"x": 132.231663451747,
5+
"y": 105.52705688973546,
6+
"z": 0.0,
7+
"rotation": {
8+
"storage": {
9+
"q0": 1.0,
10+
"q1": 0.0,
11+
"q2": 0.0,
12+
"q3": 0.0
13+
}
14+
}
15+
},
16+
"leftlegMount": {
17+
"x": 171.85973048064562,
18+
"y": 126.62958809279016,
19+
"z": 0.0,
20+
"rotation": {
21+
"storage": {
22+
"q0": 1.0,
23+
"q1": 0.0,
24+
"q2": 0.0,
25+
"q3": 0.0
26+
}
27+
}
28+
},
329
"leftWheel": {
430
"x": 66.40487960816972,
531
"y": 158.18111914980125,
@@ -14,8 +40,8 @@
1440
}
1541
},
1642
"hingingPlate": {
17-
"x": 199.83256124958473,
18-
"y": 19.95390226167468,
43+
"x": 199.90088929188147,
44+
"y": 21.90751893984761,
1945
"z": 0.0,
2046
"rotation": {
2147
"storage": {
@@ -40,8 +66,8 @@
4066
}
4167
},
4268
"CaseBottom": {
43-
"x": 77.46238442169853,
44-
"y": 111.26624312550217,
69+
"x": 77.81852553511109,
70+
"y": 110.69206740310229,
4571
"z": 0.0,
4672
"rotation": {
4773
"storage": {
@@ -66,8 +92,8 @@
6692
}
6793
},
6894
"servoCover": {
69-
"x": 83.13858373690336,
70-
"y": 8.60740351938459,
95+
"x": 85.13344935264912,
96+
"y": 9.5445536015829,
7197
"z": 0.0,
7298
"rotation": {
7399
"storage": {

0 commit comments

Comments
 (0)