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

Save before servo rebuild

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

75
Firmware 2.0/src/main.cpp
View File

@@ -1,6 +1,7 @@
#include <Arduino.h>
#include <Servo.h>
#include <EEPROM.h>
#include <ServoEasing.h>
#include "config.h"
Servo Drehung, Arm, Oberarm, Hand;
@@ -10,11 +11,53 @@ int joystick_LX_middle, joystick_LY_middle, joystick_RX_middle, joystick_RY_midd
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 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;
bool detached = true;
int eeprom = 0;
void servoWrite() {
Drehung.write(DrehungPos);
Arm.write(ArmPos);
Oberarm.write(OberarmPos);
Hand.write(HandPos);
}
void servoWrite(int DrehungP, int ArmP, int OberarmP, int HandP) {
Drehung.write(DrehungP);
Arm.write(ArmP);
Oberarm.write(OberarmP);
Hand.write(HandP);
}
int goTo(int Pos, int newPos, int time) {
float aufloesung = 1 / time * LoopTime;
float steigung = 2;
if(oldPos == -1) {
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(){
Serial.print("Get Middle Positions: ");
@@ -230,7 +273,33 @@ void detachAttach(bool detach){
}
void Autoplay(){
int DrehungPs = goTo(DrehungPos, autoAblauf[ablauf][0], 3000);
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) {
DrehungPos = DrehungPs;
}
if(ArmPs != -1) {
ArmPos = ArmPs;
}
if(OberarmPs != -1) {
OberarmPos = OberarmPs;
}
if(HandPs != -1) {
HandPos = HandPs;
}
if(DrehungPs == -1 && ArmPs == -1 && OberarmPs == -1 && HandPs == -1) {
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){
@@ -286,13 +355,11 @@ void loop(){
if(DrehungPos < DrehungMin)DrehungPos = DrehungMin;
if(ArmPos < ArmMin)ArmPos = ArmMin;
Drehung.write(DrehungPos);
Arm.write(ArmPos);
Oberarm.write(OberarmPos);
Hand.write(HandPos);
servoWrite();
}
if(offtime >= TimeToAutoplay){
detachAttach(false);
Autoplay();
}
else if(offtime >= TimeToDetach){