Lino/RoboterArm
1
0
Fork 0
mirror of https://github.com/LinoSchmidt/RoboterArm.git synced 2026-03-21 02:31:16 +01:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Autoplay

Browse Source
This commit is contained in:
Lino Schmidt
2022-05-15 13:59:51 +02:00
parent 1af707f42d
commit 18a93c78e2
2 changed files with 37 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Firmware 2.0/src/config.h
View File

@@ -51,6 +51,7 @@
//Timing //Timing
#define LoopTime 50 #define LoopTime 50
#define TimeToAutoplay 1000 #define TimeToAutoplay 1000
#define AutoplayEaseSpeed 50
#define TimeToDetach 70 #define TimeToDetach 70
#define Calibration_TimeToMiddle 3000 #define Calibration_TimeToMiddle 3000
#define Calibration_messureTime 5 #define Calibration_messureTime 5

61
Firmware 2.0/src/main.cpp
View File

@@ -15,6 +15,7 @@ int offtime = 0;
bool detached = true; bool detached = true;
int eeprom = 0; int eeprom = 0;
int autoplayLoop = 0; int autoplayLoop = 0;
bool autoplayStart = true;
int autoplayPos[][4] = { int autoplayPos[][4] = {
{20, 100, 50, 180}, {20, 100, 50, 180},
@@ -23,13 +24,35 @@ int autoplayPos[][4] = {
{50, 180, 50, 150} {50, 180, 50, 150}
}; };
void detachAttach(bool detach){
if(detach){
Drehung.detach();
Arm.detach();
Oberarm.detach();
Hand.detach();
detached = true;
}
else{
Drehung.attach(DrehungPin, DrehungPos);
Arm.attach(ArmPin, ArmPos);
Oberarm.attach(OberarmPin, OberarmPos);
Hand.attach(HandPin, HandPos);
detached = false;
}
}
void servoWrite() { void servoWrite() {
if(detached) detachAttach(false);
Drehung.write(DrehungPos); Drehung.write(DrehungPos);
Arm.write(ArmPos); Arm.write(ArmPos);
Oberarm.write(OberarmPos); Oberarm.write(OberarmPos);
Hand.write(HandPos); Hand.write(HandPos);
} }
void servoEase(int DrehungP, int ArmP, int OberarmP, int HandP, float easeTime, uint_fast8_t easeType = EASE_QUADRATIC_IN_OUT) { void servoEase(int DrehungP, int ArmP, int OberarmP, int HandP, float easeTime, uint_fast8_t easeType = EASE_QUADRATIC_IN_OUT) {
if(detached) detachAttach(false);
Drehung.setEasingType(easeType); Drehung.setEasingType(easeType);
Arm.setEasingType(easeType); Arm.setEasingType(easeType);
Oberarm.setEasingType(easeType); Oberarm.setEasingType(easeType);
@@ -236,32 +259,22 @@ void joystickButtonPress(){
} }
} }
void detachAttach(bool detach){
if(detach){
Drehung.detach();
Arm.detach();
Oberarm.detach();
Hand.detach();
detached = true;
}
else{
Drehung.attach(DrehungPin);
Arm.attach(ArmPin);
Oberarm.attach(OberarmPin);
Hand.attach(HandPin);
detached = false;
}
}
void Autoplay(){ void Autoplay(){
if(autoplayLoop == sizeof(autoplayPos)) autoplayLoop = 0; if(autoplayLoop+1 >= sizeof(autoplayPos)/sizeof(autoplayPos[0])) autoplayLoop = 0;
if(autoplayStart) {
autoplayStart = false;
servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], AutoplayEaseSpeed);
}
if(DrehungPos == autoplayPos[autoplayLoop][0] && ArmPos == autoplayPos[autoplayLoop][1] && OberarmPos == autoplayPos[autoplayLoop][2] && HandPos == autoplayPos[autoplayLoop][3]) { if(DrehungPos == autoplayPos[autoplayLoop][0] && ArmPos == autoplayPos[autoplayLoop][1] && OberarmPos == autoplayPos[autoplayLoop][2] && HandPos == autoplayPos[autoplayLoop][3]) {
servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], 50); if(autoplayLoop+1 >= sizeof(autoplayPos)/sizeof(autoplayPos[0])) {
autoplayLoop = 0;
} else {
autoplayLoop++; autoplayLoop++;
} }
servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], AutoplayEaseSpeed);
}
DrehungPos = Drehung.getCurrentAngle(); DrehungPos = Drehung.getCurrentAngle();
ArmPos = Arm.getCurrentAngle(); ArmPos = Arm.getCurrentAngle();
@@ -291,9 +304,7 @@ void loop(){
} }
else{ else{
offtime = 0; offtime = 0;
if(detached){ autoplayStart = true;
detachAttach(false);
}
if(joystick_LX != 0){ if(joystick_LX != 0){
if(InvertHand)HandPos -= joystick_LX; if(InvertHand)HandPos -= joystick_LX;
@@ -342,12 +353,10 @@ void loop(){
noTone(BuzzerPin); noTone(BuzzerPin);
} }
servoWrite(); servoWrite();
} }
if(offtime >= TimeToAutoplay){ if(offtime >= TimeToAutoplay){
detachAttach(false);
Autoplay(); Autoplay();
} }
else if(offtime >= TimeToDetach){ else if(offtime >= TimeToDetach){