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

Added ServoEasing Libary

Browse Source
This commit is contained in:
Lino Schmidt
2022-05-09 00:25:01 +02:00
parent b2c08f4c61
commit e0f82e8a31
2 changed files with 26 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Firmware 2.0/platformio.ini
View File

@@ -13,5 +13,4 @@ platform = atmelavr
board = leonardo board = leonardo
framework = arduino framework = arduino
lib_deps = lib_deps =
arduino-libraries/Servo @ ^1.1.8
arminjo/ServoEasing@^2.4.1 arminjo/ServoEasing@^2.4.1

87
Firmware 2.0/src/main.cpp
View File

@@ -1,30 +1,27 @@
#include <Arduino.h> #include <Arduino.h>
#include <Servo.h>
#include <EEPROM.h> #include <EEPROM.h>
#include <ServoEasing.h> #include <ServoEasing.hpp>
#include "config.h" #include "config.h"
Servo Drehung, Arm, Oberarm, Hand; ServoEasing Drehung, Arm, Oberarm, Hand;
int joystick_LX, joystick_LY, joystick_RX, joystick_RY; int joystick_LX, joystick_LY, joystick_RX, joystick_RY;
int joystick_LX_middle, joystick_LY_middle, joystick_RX_middle, joystick_RY_middle; int joystick_LX_middle, joystick_LY_middle, joystick_RX_middle, joystick_RY_middle;
int joystick_LX_max, joystick_LY_max, joystick_RX_max, joystick_RY_max; int joystick_LX_max, joystick_LY_max, joystick_RX_max, joystick_RY_max;
int joystick_LX_min, joystick_LY_min, joystick_RX_min, joystick_RY_min; int joystick_LX_min, joystick_LY_min, joystick_RX_min, joystick_RY_min;
int DrehungPos = Drehung_Start, ArmPos = Arm_Start, OberarmPos = Oberarm_Start, HandPos = Hand_Start; int DrehungPos = Drehung_Start, ArmPos = Arm_Start, OberarmPos = Oberarm_Start, HandPos = Hand_Start;
int oldPos = -1;
int autoAblauf[][4] = {
{135,180,111,180},
{100,120,90,100},
{100,60,50,180},
{145,180,140,180},
{135,110,131,180}
};
int ablauf = 0;
int offtime = 0; int offtime = 0;
bool detached = true; bool detached = true;
int eeprom = 0; int eeprom = 0;
int autoplayLoop = 0;
int autoplayPos[][4] = {
{20, 100, 50, 180},
{70, 140, 60, 100},
{0, 150, 0, 100},
{50, 180, 50, 150}
};
void servoWrite() { void servoWrite() {
Drehung.write(DrehungPos); Drehung.write(DrehungPos);
@@ -32,30 +29,16 @@ void servoWrite() {
Oberarm.write(OberarmPos); Oberarm.write(OberarmPos);
Hand.write(HandPos); Hand.write(HandPos);
} }
void servoWrite(int DrehungP, int ArmP, int OberarmP, int HandP) { void servoEase(int DrehungP, int ArmP, int OberarmP, int HandP, float easeTime, uint_fast8_t easeType = EASE_QUADRATIC_IN_OUT) {
Drehung.write(DrehungP); Drehung.setEasingType(easeType);
Arm.write(ArmP); Arm.setEasingType(easeType);
Oberarm.write(OberarmP); Oberarm.setEasingType(easeType);
Hand.write(HandP); Hand.setEasingType(easeType);
}
int goTo(int Pos, int newPos, int time) {
float aufloesung = 1 / time * LoopTime; Drehung.easeTo(DrehungP, easeTime);
float steigung = 2; Arm.easeTo(ArmP, easeTime);
Oberarm.easeTo(OberarmP, easeTime);
if(oldPos == -1) { Hand.easeTo(HandP, easeTime);
oldPos = Pos;
}
float sendPos = (int)((pow(aufloesung, steigung) / pow(aufloesung, steigung) + pow((1 - aufloesung), steigung)) + oldPos) + (newPos - oldPos - 1)*Pos;
if(sendPos == newPos) {
oldPos = -1;
return -1;
}
return sendPos;
} }
void calibrateMiddle(){ void calibrateMiddle(){
@@ -273,33 +256,15 @@ void detachAttach(bool detach){
} }
void Autoplay(){ void Autoplay(){
int DrehungPs = goTo(DrehungPos, autoAblauf[ablauf][0], 3000); if(autoplayLoop == sizeof(autoplayPos)) autoplayLoop = 0;
int ArmPs = goTo(ArmPos, autoAblauf[ablauf][1], 3000);
int OberarmPs = goTo(OberarmPos, autoAblauf[ablauf][2], 3000);
int HandPs = goTo(HandPos, autoAblauf[ablauf][3], 3000);
if(DrehungPs != -1) { servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], 50);
DrehungPos = DrehungPs; DrehungPos = Drehung.getCurrentAngle();
} ArmPos = Arm.getCurrentAngle();
if(ArmPs != -1) { OberarmPos = Oberarm.getCurrentAngle();
ArmPos = ArmPs; HandPos = Hand.getCurrentAngle();
}
if(OberarmPs != -1) {
OberarmPos = OberarmPs;
}
if(HandPs != -1) {
HandPos = HandPs;
}
if(DrehungPs == -1 && ArmPs == -1 && OberarmPs == -1 && HandPs == -1) { autoplayLoop++;
if(ablauf < sizeof(autoAblauf)) {
ablauf++;
} else {
ablauf = 0;
}
}
servoWrite();
} }
int joystick_position(int joystick, int joystick_middle, int joystick_min, int joystick_max, int joystick_MinSpeed, int joystick_MaxSpeed){ int joystick_position(int joystick, int joystick_middle, int joystick_min, int joystick_max, int joystick_MinSpeed, int joystick_MaxSpeed){