|
www.elektronik.si Forum o elektrotehniki in računalništvu
|
Poglej prejšnjo temo :: Poglej naslednjo temo |
Avtor |
Sporočilo |
igi82m Neznanec
Pridružen-a: Pon 09 Avg 2021 18:14 Prispevkov: 5 Aktiv.: 0.15
|
Objavljeno: Pon Avg 09, 2021 7:04 pm Naslov sporočila: Pomoč pri arduino kodi za robotsko roko |
|
|
Pozdravljeni , ker sem bolj začetnik v tem ,bi rabil pomoč pri arduino kodi za robotsko roko z uporabo joystika in shranjevanje pozicije. Spletna stran z kodo katero uporabljam.
MeArm Robot
Težava je v tem , da jaz uporabljam 3 joystike in 6 servotov , koda pa je za 2 joystika in 4 servote. Kodo sem kolikor sem znal spremenil tako , da zadeva z joystiki in servoti ročno deluje. Zdaj je težava v shranjevanju pozicij , v serial monitorju mi pokaže 6 vrstic za vsak servo in tudi pokaže vrednosti ko premikam joystik tako kot mora. Problem je , da mi pozicijo dveh dodanih servotov ne shrani na pravo vrstico , oziroma shranjuje dvakrat na isto mesto. Kaj bi moral dodat oziroma spremenit v kodi?
Hvala.
Koda: |
/*
meArm analog joysticks version 1.5.4 - UtilStudio.com Dec 2018
Uses two analogue joysticks and four servos.
In version 1.5.4 was replaced an external resistors by internal pull up resistors.
External LED diode was replaced with using internal "L" diode on Arduino UNO board.
Some bugs was removed.
First joystick moves gripper forwards, backwards, left and right,
button start/stop recording positions.
Second joystick moves gripper up, down, and closes and opens,
button start/stop playing recorded positions.
Press button for 2 seconds to autoplay.
Pins:
Arduino Stick1 Stick2 Base Shoulder Elbow Gripper Record/
GND GND GND Brown Brown Brown Brown Auto play
5V VCC VCC Red Red Red Red LED
A0 HOR
A1 VER
PD2 BUTT
A2 HOR
A3 VER
PD3 BUTT
11 Yellow
10 Yellow
9 Yellow
6 Yellow
13 X
*/
bool repeatePlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */
int basePin = 11; /* Base servo */
int shoulderPin = 10; /* Shoulder servo */
int elbowPin = 9; /* Elbow servo */
int gripperPin = 6; /* Gripper servo */
int forearmPin = 5; /* ForeArm servo */
int wristPin = 4; /* Wrist servo*/
int xdirPin = 0; /* Base - joystick1*/
int ydirPin = 1; /* Shoulder - joystick1 */
int zdirPin = 3; /* Elbow - joystick2 */
int gdirPin = 2; /* Gripper - joystick2 */
int sdirPin = 4; /* ForeArm - joystick3 */
int rdirPin = 5; /* Wrist - joystick3 */
//int pinRecord = A4; /* Button record - backward compatibility */
//int pinPlay = A5; /* Button play - backward compatibility */
int pinRecord = PD2; /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay = PD3; /* Button play - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = 13; /* LED - indicates recording (light) or auto play mode (blink ones) */
const int buffSize = 512; /* Size of recording buffer */
int startBase = 135;
int startShoulder = 95;
int startElbow = 90;
int startGripper = 0;
int startForeArm = 155;
int startWrist = 150;
int posBase = 90;
int posShoulder = 90;
int posElbow = 90;
int posGripper = 0;
int posForeArm = 90;
int posWrist = 90;
int lastBase = 90;
int lastShoulder = 90;
int lastElbow = 90;
int lastGripper = 0;
int lastForeArm = 90;
int lastWrist = 90;
int minBase = 10;
int maxBase = 150;
int minShoulder = 10;
int maxShoulder = 150;
int minElbow = 0;
int maxElbow = 180;
int minGripper = 100;
int maxGripper = 150;
int minForeArm = 0;
int maxForeArm = 180;
int minWrist = 0;
int maxWrist = 180;
const int countServo = 6;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;
int buttonRecord = HIGH;
int buttonPlay = HIGH;
int buttonRecordLast = LOW;
int buttonPlayLast = LOW;
bool record = false;
bool play = false;
bool debug = false;
String command = "Manual";
int printPos = 0;
int buttonPlayDelay = 20;
int buttonPlayCount = 0;
bool ledLight = false;
Servo servoBase;
Servo servoShoulder;
Servo servoElbow;
Servo servoGripper;
Servo servoForeArm;
Servo servoWrist;
void setup() {
Serial.begin(9600);
pinMode(xdirPin, INPUT);
pinMode(ydirPin, INPUT);
pinMode(zdirPin, INPUT);
pinMode(gdirPin, INPUT);
pinMode(sdirPin, INPUT);
pinMode(rdirPin, INPUT);
pinMode(pinRecord, INPUT_PULLUP);
pinMode(pinPlay, INPUT_PULLUP);
pinMode(pinLedRecord, OUTPUT);
servoBase.attach(basePin);
servoShoulder.attach(shoulderPin);
servoElbow.attach(elbowPin);
servoGripper.attach(gripperPin);
servoForeArm.attach(forearmPin);
servoWrist.attach(wristPin);
StartPosition();
digitalWrite(pinLedRecord, HIGH);
delay(1000);
digitalWrite(pinLedRecord, LOW);
}
void loop() {
buttonRecord = digitalRead(pinRecord);
buttonPlay = digitalRead(pinPlay);
// Serial.print(buttonRecord);
// Serial.print("\t");
// Serial.println(buttonPlay);
// for testing purposes
if (buttonPlay == LOW)
{
buttonPlayCount++;
if (buttonPlayCount >= buttonPlayDelay)
{
repeatePlaying = true;
}
}
else buttonPlayCount = 0;
if (buttonPlay != buttonPlayLast)
{
if (record)
{
record = false;
}
if (buttonPlay == LOW)
{
play = !play;
repeatePlaying = false;
if (play)
{
StartPosition();
}
}
}
if (buttonRecord != buttonRecordLast)
{
if (buttonRecord == LOW)
{
record = !record;
if (record)
{
play = false;
repeatePlaying = false;
recPos = 0;
}
else
{
if (debug) PrintBuffer();
}
}
}
buttonPlayLast = buttonPlay;
buttonRecordLast = buttonRecord;
float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
float dy = map(analogRead(ydirPin), 0, 1023, -5.0, 5.0);
float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);
float ds = map(analogRead(sdirPin), 0, 1023, -5.0, 5.0);
float dr = map(analogRead(rdirPin), 0, 1023, -5.0, 5.0);
if (abs(dx) < 1.5) dx = 0;
if (abs(dy) < 1.5) dy = 0;
if (abs(dz) < 1.5) dz = 0;
if (abs(dg) < 1.5) dg = 0;
if (abs(ds) < 1.5) ds = 0;
if (abs(dr) < 1.5) dr = 0;
posBase += dx;
posShoulder += dy;
posElbow += dz;
posGripper += dg;
posForeArm += ds;
posWrist += dr;
if (play)
{
if (playPos >= recPos) {
playPos = 0;
if (repeatePlaying)
{
delay(delayBetweenCycles);
StartPosition();
}
else
{
play = false;
}
}
bool endOfData = false;
while (!endOfData)
{
if (playPos >= buffSize - 1) break;
if (playPos >= recPos) break;
int data = buff[playPos];
int angle = data & 0xFFF;
int servoNumber = data & 0x5000;
endOfData = data & 0x6000;
switch (servoNumber)
{
case 0x0000:
posBase = angle;
break;
case 0x1000:
posShoulder = angle;
break;
case 0x2000:
posElbow = angle;
break;
case 0x3000:
posGripper = angle;
break;
case 0x4000:
posForeArm = angle;
break;
case 0x5000:
posWrist = angle;
ds = posWrist - lastWrist;
break;
}
playPos++;
}
}
if (posBase > maxBase) posBase = maxBase;
if (posShoulder > maxShoulder) posShoulder = maxShoulder;
if (posElbow > maxElbow) posElbow = maxElbow;
if (posGripper > maxGripper) posGripper = maxGripper;
if (posForeArm > maxForeArm) posForeArm = maxForeArm;
if (posWrist > maxWrist) posWrist = maxWrist;
if (posBase < minBase) posBase = minBase;
if (posShoulder < minShoulder) posShoulder = minShoulder;
if (posElbow < minElbow) posElbow = minElbow;
if (posGripper < minGripper) posGripper = minGripper;
if (posForeArm < minForeArm) posForeArm = minForeArm;
if (posWrist < minWrist) posWrist = minWrist;
servoBase.write(posBase);
servoShoulder.write(posShoulder);
servoElbow.write(posElbow);
servoForeArm.write(posForeArm);
servoWrist.write(posWrist);
// if (dg < -3.0) {
// posGripper = minGripper;
// servoGripper.write(posGripper);
// Serial.println(posGripper);
// }
// else if (dg > 3.0) {
// posGripper = maxGripper;
// servoGripper.write(posGripper);
// Serial.println(posGripper);
// }
bool waitGripper = false;
if (dg < 0) {
posGripper = minGripper;
waitGripper = true;
}
else if (dg > 0) {
posGripper = maxGripper;
waitGripper = true;
}
servoGripper.write(posGripper);
if (play && waitGripper)
{
delay(1000);
}
//Serial.println(posGripper);
if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper) | (lastForeArm != posForeArm) | (lastWrist != posWrist))
{
if (record)
{
if (recPos < buffSize - countServo)
{
int buffPos = 0;
if (lastBase != posBase)
{
buffAdd[buffPos] = posBase;
buffPos++;
}
if (lastShoulder != posShoulder)
{
buffAdd[buffPos] = posShoulder | 0x1000;
buffPos++;
}
if (lastElbow != posElbow)
{
buffAdd[buffPos] = posElbow | 0x2000;
buffPos++;
}
if (lastGripper != posGripper)
{
buffAdd[buffPos] = posGripper | 0x3000;
buffPos++;
}
if (lastForeArm != posForeArm)
{
buffAdd[buffPos] = posForeArm | 0x4000;
buffPos++;
}
if (lastWrist != posWrist)
{
buffAdd[buffPos] = posWrist | 0x5000;
buffPos++;
}
buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x6000;
for (int i = 0; i < buffPos; i++)
{
buff[recPos + i] = buffAdd[i];
}
recPos += buffPos;
}
}
command = "Manual";
printPos = 0;
if (play)
{
command = "Play";
printPos = playPos;
}
else if (record)
{
command = "Record";
printPos = recPos;
}
Serial.print(command);
Serial.print("\t");
Serial.print(printPos);
Serial.print("\t");
Serial.print(posBase);
Serial.print("\t");
Serial.print(posShoulder);
Serial.print("\t");
Serial.print(posElbow);
Serial.print("\t");
Serial.print(posGripper);
Serial.print("\t");
Serial.print(posForeArm);
Serial.print("\t");
Serial.print(posWrist);
Serial.print("\t");
Serial.print(record);
Serial.print("\t");
Serial.print(play);
Serial.println();
}
lastBase = posBase;
lastShoulder = posShoulder;
lastElbow = posElbow;
lastGripper = posGripper;
lastForeArm = posForeArm;
lastWrist = posWrist;
if ( repeatePlaying)
{
ledLight = !ledLight;
}
else
{
if (ledLight)
{
ledLight = false;
}
if (record)
{
ledLight = true;
}
};
digitalWrite(pinLedRecord, ledLight);
delay(50);
}
void PrintBuffer()
{
for (int i = 0; i < recPos; i++)
{
int data = buff[i];
int angle = data & 0xFFF;
int servoNumber = data & 0x5000;
bool endOfData = data & 0x6000;
Serial.print("Servo=");
Serial.print(servoNumber);
Serial.print("\tAngle=");
Serial.print(angle);
Serial.print("\tEnd=");
Serial.print(endOfData);
Serial.print("\tData=");
Serial.print(data, BIN);
Serial.println();
}
}
void StartPosition()
{
int angleBase = servoBase.read();
int angleShoulder = servoShoulder.read();
int angleElbow = servoElbow.read();
int angleGripper = servoGripper.read();
int angleForeArm = servoForeArm.read();
int angleWrist = servoWrist.read();
Serial.print(angleBase);
Serial.print("\t");
Serial.print(angleShoulder);
Serial.print("\t");
Serial.print(angleElbow);
Serial.print("\t");
Serial.print(angleGripper);
Serial.print("\t");
Serial.print(angleForeArm);
Serial.print("\t");
Serial.print(angleWrist);
Serial.println("\t");
posBase = startBase;
posShoulder = startShoulder;
posElbow = startElbow;
posGripper = startGripper;
posForeArm = startForeArm;
posWrist = startWrist;
servoBase.write(posBase);
servoShoulder.write(posShoulder);
servoElbow.write(posElbow);
servoGripper.write(posGripper);
servoWrist.write(posWrist);
} |
|
|
Nazaj na vrh |
|
|
airwolf Član
Pridružen-a: Sre 28 Maj 2008 17:17 Prispevkov: 2060 Aktiv.: 10.65
|
Objavljeno: Tor Avg 10, 2021 12:58 pm Naslov sporočila: |
|
|
Premajhen buffer?
const int buffSize = 512; /* Size of recording buffer */ _________________ LP Gašper |
|
Nazaj na vrh |
|
|
igi82m Neznanec
Pridružen-a: Pon 09 Avg 2021 18:14 Prispevkov: 5 Aktiv.: 0.15
|
Objavljeno: Tor Avg 10, 2021 4:02 pm Naslov sporočila: |
|
|
Za koliko ga lahko še povečam? |
|
Nazaj na vrh |
|
|
mosqito Član
Pridružen-a: Čet 07 Apr 2016 23:22 Prispevkov: 3234 Aktiv.: 33.07
|
Objavljeno: Tor Avg 10, 2021 5:46 pm Naslov sporočila: |
|
|
Poglej, kaj izpiše prevajalnik na koncu za RAM. _________________ Always going the extra mile. |
|
Nazaj na vrh |
|
|
igi82m Neznanec
Pridružen-a: Pon 09 Avg 2021 18:14 Prispevkov: 5 Aktiv.: 0.15
|
Objavljeno: Tor Avg 10, 2021 6:03 pm Naslov sporočila: |
|
|
Sem ga še malo povečal , na 96%. Pa ne pomaga , še vedno isto. Kaj bi še lahko probal? |
|
Nazaj na vrh |
|
|
mato1111 Član
Pridružen-a: Pet 28 Dec 2012 14:42 Prispevkov: 612 Aktiv.: 4.45 Kraj: Vrhnika
|
Objavljeno: Tor Avg 10, 2021 8:40 pm Naslov sporočila: |
|
|
Koda: |
buffAdd[buffPos] = posWrist | 0x5000; |
Zakaj v tem primeru bitni OR z 0x5000? (ali pa 0x1000, 0x2000, ...)
PS:
Mi je že jasno... neke vrste "koda"ki se zapiše pred vrednost pozicije po kateri program ve kateri sklep je treba premaknit ko bere ven iz buferja... |
|
Nazaj na vrh |
|
|
igi82m Neznanec
Pridružen-a: Pon 09 Avg 2021 18:14 Prispevkov: 5 Aktiv.: 0.15
|
Objavljeno: Sre Avg 11, 2021 7:17 pm Naslov sporočila: |
|
|
Samo , zakaj ne shrani oziroma bere iz pravega? |
|
Nazaj na vrh |
|
|
mato1111 Član
Pridružen-a: Pet 28 Dec 2012 14:42 Prispevkov: 612 Aktiv.: 4.45 Kraj: Vrhnika
|
Objavljeno: Sre Avg 11, 2021 9:38 pm Naslov sporočila: |
|
|
igi82m je napisal/a: |
Samo , zakaj ne shrani oziroma bere iz pravega? |
Malo preglej program da kje ne bereš/pišeš 2x al pa če po nesreči zapišeš v napačnega. |
|
Nazaj na vrh |
|
|
igi82m Neznanec
Pridružen-a: Pon 09 Avg 2021 18:14 Prispevkov: 5 Aktiv.: 0.15
|
Objavljeno: Pon Avg 16, 2021 6:00 pm Naslov sporočila: |
|
|
Težave se začnejo , ko dodam te vrednosti za dodatna servota (ForeArm 0x4000 in Wrist 0x5000). Takrat se mi shranjevanje vse zameša. V čem je problem?
Moja koda 6xservo 3xJoy
int data = buff[playPos];
int angle = data & 0xFFF;
int servoNumber = data & 0x5000;
endOfData = data & 0x6000;
switch (servoNumber)
{
case 0x0000:
posBase = angle;
break;
case 0x1000:
posShoulder = angle;
break;
case 0x2000:
posElbow = angle;
break;
case 0x3000:
posGripper = angle;
break;
case 0x4000:
posForeArm = angle;
break;
case 0x5000:
posWrist = angle;
ds = posWrist - lastWrist;
break;
if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper) | (lastForeArm != posForeArm) | (lastWrist != posWrist))
{
if (record)
{
if (recPos < buffSize - countServo)
{
int buffPos = 0;
if (lastBase != posBase)
{
buffAdd[buffPos] = posBase;
buffPos++;
}
if (lastShoulder != posShoulder)
{
buffAdd[buffPos] = posShoulder | 0x1000;
buffPos++;
}
if (lastElbow != posElbow)
{
buffAdd[buffPos] = posElbow | 0x2000;
buffPos++;
}
if (lastGripper != posGripper)
{
buffAdd[buffPos] = posGripper | 0x3000;
buffPos++;
}
if (lastForeArm != posForeArm)
{
buffAdd[buffPos] = posForeArm | 0x4000;
buffPos++;
}
if (lastWrist != posWrist)
{
buffAdd[buffPos] = posWrist | 0x5000;
buffPos++;
}
buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x6000;
Orginal koda za 4xservo 2xJoy
int data = buff[playPos];
int angle = data & 0xFFF;
int servoNumber = data & 0x3000;
endOfData = data & 0x4000;
switch (servoNumber)
{
case 0x0000:
posBase = angle;
break;
case 0x1000:
posShoulder = angle;
break;
case 0x2000:
posElbow = angle;
break;
case 0x3000:
posGripper = angle;
ds = posGriper - lastGriper;
break;
if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper))
{
if (record)
{
if (recPos < buffSize - countServo)
{
int buffPos = 0;
if (lastBase != posBase)
{
buffAdd[buffPos] = posBase;
buffPos++;
}
if (lastShoulder != posShoulder)
{
buffAdd[buffPos] = posShoulder | 0x1000;
buffPos++;
}
if (lastElbow != posElbow)
{
buffAdd[buffPos] = posElbow | 0x2000;
buffPos++;
}
if (lastGripper != posGripper)
{
buffAdd[buffPos] = posGripper | 0x3000;
buffPos++;
}
buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x4000; |
|
Nazaj na vrh |
|
|
mosqito Član
Pridružen-a: Čet 07 Apr 2016 23:22 Prispevkov: 3234 Aktiv.: 33.07
|
Objavljeno: Tor Avg 17, 2021 8:16 am Naslov sporočila: |
|
|
Naslednjič objavi bolj čitljivo kodo in uporabi oblikovanje s [ code ]:
Koda: |
int data = buff[playPos];
int angle = data &0xFFF;
int servoNumber = data &0x5000;
endOfData = data &0x6000;
switch (servoNumber)
{
case 0x0000:
posBase = angle;
break;
case 0x1000:
posShoulder = angle;
break;
case 0x2000:
posElbow = angle;
break;
case 0x3000:
posGripper = angle;
break;
case 0x4000:
posForeArm = angle;
break;
case 0x5000:
posWrist = angle;
ds = posWrist - lastWrist;
break;
if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper) | (lastForeArm != posForeArm) | (lastWrist != posWrist))
{
if (record)
{
if (recPos < buffSize - countServo)
{
int buffPos = 0;
if (lastBase != posBase)
{
buffAdd[buffPos] = posBase;
buffPos++;
}
if (lastShoulder != posShoulder)
{
buffAdd[buffPos] = posShoulder | 0x1000;
buffPos++;
}
if (lastElbow != posElbow)
{
buffAdd[buffPos] = posElbow | 0x2000;
buffPos++;
}
if (lastGripper != posGripper)
{
buffAdd[buffPos] = posGripper | 0x3000;
buffPos++;
}
if (lastForeArm != posForeArm)
{
buffAdd[buffPos] = posForeArm | 0x4000;
buffPos++;
}
if (lastWrist != posWrist)
{
buffAdd[buffPos] = posWrist | 0x5000;
buffPos++;
}
buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x6000;
|
...
Tule imaš ustrezno "orodje":
https://techiedelight.com/tools/clike
Gornje oblikovanje sicer ni najlepše. Predlagam, da si pogledaš še tole:
https://github.com/MaJerle/c-code-style _________________ Always going the extra mile. |
|
Nazaj na vrh |
|
|
airwolf Član
Pridružen-a: Sre 28 Maj 2008 17:17 Prispevkov: 2060 Aktiv.: 10.65
|
Objavljeno: Tor Avg 17, 2021 8:21 am Naslov sporočila: |
|
|
int data = buff[playPos];
int angle = data &0xFFF;
int servoNumber = data &0x3000;
endOfData = data &0x4000;
Ali nebi slučajno moral biti tale:
endOfData = data &0x4000;
takole:
endOfData = data &0x6000;
Edit sem videl da je to stara koda (preden si jo predelal za 6 servotov), ignoriraj. _________________ LP Gašper |
|
Nazaj na vrh |
|
|
|
|
Ne, ne moreš dodajati novih tem v tem forumu Ne, ne moreš odgovarjati na teme v tem forumu Ne, ne moreš urejati svojih prispevkov v tem forumu Ne, ne moreš brisati svojih prispevkov v tem forumu Ne ne moreš glasovati v anketi v tem forumu Ne, ne moreš pripeti datotek v tem forumu Ne, ne moreš povleči datotek v tem forumu
|
Uptime: 69 dni
Powered by phpBB © 2001, 2005 phpBB Group
|