Zdalnie sterowana platforma robota z napędem gąsienicowym + 6-osiowy manipulator. Całkowicie autorska konstrukcja, wszystkie elementy, w tym gąsienice, są wydrukowane na drukarce 3d. Czołg świetnie radzi sobie nawet w bardzo trudnym terenie. Jest to konstrukcja modularna, czołg można zmontować bez ramienia, wewnątrz jest bardzo dużo miejsca na dodatkowy akumulator i więcej elektroniki. Czołg jest zdalnie sterowany z pada do PS4, ale można łatwo zmodyfikować kod do współpracy z innym kontrolerem.
Czołg (bez ramienia)
ELEKTRONIKA:
1. ESP-WROOM-32 https://botland.com.pl/esp32/8893-esp32-wifi-bt-42-platforma-z-modulem-esp-wroom-32-zgodny-z-esp32-devkit-5904422337438.html
2. Cytron MDD3A (jest lepszy niż sterownik L298N) https://botland.com.pl/sterowniki-silnikow-moduly/15819-cytron-mdd3a-dwukanalowy-sterownik-silnikow-dc-16v3a-5904422324841.html
3. Silnik DC z przekładnią (Moje silniki mają 300RPM, niestety zostały wycofane z botlandu, link prowadzi do zamiennika o podobnych wymiarach) https://botland.com.pl/silniki-dc-standard-z-przekladnia/22685-silnik-z-przekladnia-601-6v-130rpm.html
4. Breakout board do ESP-WROOM-32 (https://elektroweb.pl/pl/plytki-rozszerzen-do-esp8266-i-esp32/1314-plytka-rozszerzen-dla-esp32-devkitc-1-v1.html?gad_source=1&gad_campaignid=21647297689&gbraid=0AAAAADsUy2bUuTErYpIVpWAk4uOBgANES&gclid=CjwKCAjw6MPRBhBTEiwAd-7Mr2OMVMYLmdc0-MTZbL-GMPA7yG6cqx1LArEnD6xqTK0lvAtEwL_j3xoCQKwQAvD_BwE)
5. Akumulator NiMh 7,2V 3000mAH
6. Włącznik/wyłącznik (Warto mieć bo odpinanie konektora do baterii nie jest proste w nagłych sytuacjach, link jest do przykładowego który będzie działał) https://botland.com.pl/przelaczniki-kolyskowe-dwupozycyjne-3-pozycyjne/6686-wylacznik-on-off-irs-101-8c-d-12vdc-20a-z-podswietleniem-czerwony-5904422334932.html
7. L7805CV (stabilizator 5V do zasilenia płytki)
8. OPCJONALNIE listwy LED https://botland.com.pl/lancuchy-i-matryce-led/16152-listwa-led-rgb-ws2812-5050-x-8-diod-53mm-wlutowane-zlacza-5904422325398.html
9. OPCJONALNIE konektory do przycisków, dzięki nim nie trzeba lutować włącznika, który jest bardzo łatwy do stopienia https://botland.com.pl/zakonczenia-przewodow/21968-zestaw-konektorow-plaskich-zlote-i-srebrne-koszulki-izolacyjne-150szt-justpi-5904422346898.html
MECHANIKA:
1. TPU do gąsienic https://botland.com.pl/index.php?controller=order-detail&id_order=1102807
2. Łożyska 625zz (5x16x5mm) - zawieszenie
3. Łożyska 6803 2RS - dodatkowe wzmocnienie koła napędowego
4. Sprężyny naciągowe https://www.leroymerlin.pl/produkty/sprezyna-naciagowa-1-kg-7x45x0-7-mm-4-sztuki-standers-91007864.html
5. Wszystkie śruby są M3, w leory merlin mają fajne paczki z śrubkami i nakrętkami.
6. PET-G do wydruków
RAMIĘ (Można je zbudować osobno):
ELEKTRONIKA:
1. Sterownik Servo I2C pca9685
2. Przetwornica LM2596 x2 https://sklep.msalamon.pl/produkt/przetwornica-3a-dc-dc-step-down-lm2596/?gad_source=1&gad_campaignid=21700502614&gbraid=0AAAAAByhW9D_LOnZrX8p6D5wCCXDuvoMc&gclid=CjwKCAjw6MPRBhBTEiwAd-7MryyAJngZo6ZH5WH2jQGrGCYNcolbYaR9K1AWgHC9t8yhplHujCecVRoCVNAQAvD_BwE
3. Servo 20kg Surpass S2000ML lub zamiennik
4. 5x Servo MG996R KONIECZNIE Z METALOWYMI ZĘBATKAMI https://abc-rc.pl/pl/products/serwo-cyfrowe-mg996r-55g-13kg-cm-high-quality-full-metalowe-zebatki-9462.html
5. Silnik DC z enkoderem https://botland.com.pl/silniki-dc-z-przekladnia-i-enkoderami/6287-silnik-z-przekladnia-sj01-120-1-6v-160rpm-enkoder-6959420910205.html
6. 2x Czujnik krańcowy KY-003 https://botland.com.pl/czujniki-pradu/14290-modul-z-czujnikiem-halla-pola-magnetycznego-5903351241960.html?cd=22829422733&ad=&kd=&gad_source=1&gad_campaignid=22833256297&gbraid=0AAAAADDatXiEO_oMa6TuwgbVTS6ZD7ALH&gclid=CjwKCAjw6MPRBhBTEiwAd-7Mr-4FJYlo85-H6EW4-ARSGYRohQ58pGXIPAZUZE0WG6Llk5em7N8g3BoCH1UQAvD_BwE
7. L293d lub inny nieduży sterownik silnika DC
8. OPCJONALNIE Pierścień ślizgowy 12-przewodowy z aliexpress
9. OPCJONALNIE Przedłużacze Serwo
MECHANIKA:
1. Łożyska 625zz (5x16x5mm) w podstawie ramienia
2. Łożyska 6803 2RS - w stawie "ramienia" manipulatora
3. Śruby M5x16 do przykręcenia łożysk 625zz
4. 1x magnes neodymowy wymiary przynajmniej 3x3x2
5. Pasek GT2 6mm 550mm
6. Łożysko 6708 2rs - w podstawie manipulatora
1. Lutownica
2. Drukarka 3D
3. Program CAD
4. OPCJONALNIE Zaciskarka do kabli'
Zdalnie sterowany czołg - W całości wydrukowany na drukarce 3d. Układ jezdny składa się z gąsienic wydrukowanych z TPU, zawieszenie na sprężynach naciągowych i napinaczy gąsienicy (można go pominąć w mniejszych konstrukcjach). Każde koło zamontowane jest na łożysku. Gąsienice z TPU są praktycznie niezniszczalne, i jednocześnie gwarantują dobrą przyczepność. Polecam wymodelować przejściówkę między czołgiem a kołem napinającym (tym przednim). W ten sposób można łatwo dopasować napięcie gąsienicy do wydruku, który mógł się skurczyć. Nie powinna być bardzo ciasna ale zbyt duży luz sprawi że spadnie. Należy znaleźć złoty środek i poeksperymentować. Tylne koło napędowe może wymagać dodatkowego wspornika który trzyma je od zewnątrz w zależności od tego jaki luz ma wał silnika.
Polecam zamontować silniki z tyłu, bo z przodu będzie więcej miejsca na czujniki itd. Akumulator powinien być możliwie nisko żeby środek ciężkości był na dole, co jest ważne jeśli na górę chcemy wrzucić ciężkie ramię robota. Podzielenie czołu na przedziały (jak w prawdziwych czołgach) może być pomocne. Te przedziały to: napędowy (silniki i sterownik), bateria (na baterię) i elektronika sterująca. Należy zadbać o łatwy dostęp do USB na mikrokontrolerze, gdy będzie w czołgu :)
Niestety nie dołączam do opisu plików .stl ze względu na obszerność projektu. Czołg jest ciągle rozwijany i nie posiada wersji gotowej do wydruku. Drukowałem .30mm z infillem 20% z PET-G.
Czołg sterowany jest z ESP32-WROOM które łączy się z padem do PS4 przez bibliotekę <PS4Controller.h>. Kod czołgu działa na bazie automatu stanu, posiadającego tryb jazdy, tryb ruszania ramieniem i tryb awaryjny gdy straci komunikację. Sterowanie jazdą jest bardzo intuicyjne, jest inspirowane zachowaniem czołgu z World of Tanks Blitz. Dokładny sposób działania można znaleźć w kodzie.
Ramię jest obracane przez silnik DC z enkoderem przez pasek GT2. Kalibracja ramienia odbywa się za pomocą dwóch czujników krańcowych przyczepionych do czołgu które wykrywają magnes przyczepiony do podstawy ramienia. Komunikacja z resztą ramienia odbywa się przez I2C które prowadzi do sterownika serw. Zasilanie oraz magistrala I2C są doprowadzone do ramienia przez pierścień ślizgowy co zapewnia nieograniczony obrót ramienia. Wadą tego rozwiązania jest ograniczenie połączeń które można przeprowadzić między ramieniem a czołiem, w tej chwili nie można doczepić już nic więcej. Ramię sterowane jest w trybie open-loop a jako że pędkością serwosilników nie można sterować ruch odbywa się inkrementalnie w każdej pętli programu. Aktualnie programuję reverse kinematics do ramienia, w kodzie możliwe jest sterowanie pojedynczymi członami na raz. Ramię podniesie bez problemów szczura z IKEI nawet 30 cm od bazy.
Elektronika: z baterii przez L7805CV 5V idzie na VIN do breakout boarda esp32 i ledów. Z przetwornic uzyskuję 7V i 6V które idą do serw w ramieniu. Bateria też idzie bezpośrednio do sterownika silników DC.
// Kod do czołgu z ramieniem
// Autor: Filip Maziarka filip1maziarka@gmail.com
// Kod jest mocno prowizoryczny, ale działa. Najfajniejsze jest sterowanie jazdą bo jest bardzo intuicyjne ale w tym kodzie głównie są jakieś proste rzeczy.
#include <PS4Controller.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <ESP32Encoder.h>
#include <Adafruit_NeoPixel.h>
const int frontLedPin = 17, rearLedPin = 15;
#define NUMPIXELS 8
Adafruit_NeoPixel frontLed(NUMPIXELS, frontLedPin, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel rearLed(NUMPIXELS, rearLedPin, NEO_GRB + NEO_KHZ800);
int idleLed = 2;
int firstpass = 0;
int rainbowHue = 0;
void rainbow() {
frontLed.rainbow(rainbowHue, 1, 255, 50);
rearLed.rainbow(rainbowHue, 1, 255, 50);
rainbowHue+=256;
if(rainbowHue>5*65536)
{
rainbowHue = 0;
frontLed.show();
rearLed.show();
}
}
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
ESP32Encoder encoder;
int servoSelector = 0;
const int fullTurn = 21833;
double clampFloat(double ff,double fm)
{
if(ff > fm)
{
ff-=2*fm;
}
if(ff < -fm)
{
ff += 2*fm;
}
return ff;
}
double dead(double ff, double fd)
{
if(ff < fd&&ff > -fd)
{
return 0.0;
}
return ff;
}
double mapFloat(double fx, double low1, double high1, double low2, double high2)
{
fx = constrain(fx, low1, high1);
fx = low2 + (high2-low2)*(fx-low1)/(high1-low1);
return fx;
}
double vectorLength(double fx[])
{
double fout = 0;
for(int i = 0; i < 3; i++)
{
fout+=sq(fx[i]);
}
return sqrt(fout);
}
double dotProduct(double fv1[], double fv2[])
{
double fout = 0;
for(int i = 0; i < 3; i++)
{
fout += fv1[i]*fv2[i];
}
return fout;
}
void crossProduct(double fv1[], double fv2[], double fout[])
{
fout[0] = fv1[1]*fv2[2]-fv1[2]*fv2[1];
fout[1] = fv1[2]*fv2[0]-fv1[0]*fv2[2];
fout[2] = fv1[0]*fv2[1]-fv1[1]*fv2[0];
}
const double pi = 3.141592;
int lastGrabTime = 0;
int hasGrabbed = 0;
int lastPosTime = 0;
double simpleAngles[6] = {0,pi,0,0,0,0};
const int enc1 = 25, enc2 = 26, endPin1 = 27, endPin2 = 14;
int angles[6] = {2260,2000,1400,1850,1400,2350};
int mode = 1;
int modeSwitchStart = 0;
int modeSwitchHold = 0;
int modeSwitched = 0;
int L3R3Timer = 0;
int e1 = 0, e2 = 0;
volatile int d2 = 0;
volatile int dist = 0;
const int m1a = 19, m1b = 18, m2a= 16, m2b = 4;
double toRad(double fx)
{
return fx*2*pi/360;
}
void moveServo(int fIndex, int fAng)
{
fIndex = constrain(fIndex, 0, 5);
pwm.writeMicroseconds(fIndex, fAng);
}
void angleServo(int fIndex, double fAng)
{
double sAngle = 0;
if(fIndex == 0)//elbow
{
fAng = constrain(fAng, 0, pi);
sAngle = mapFloat(fAng, 0, pi, 2260, 460);
//sAngle = constrain(fAng, 620, 2260);
pwm.writeMicroseconds(fIndex, sAngle);
}
else if(fIndex == 5)//arm
{
fAng = constrain(fAng, 0, pi);
sAngle = mapFloat(fAng, 0, pi, 600, 2350);
pwm.writeMicroseconds(fIndex, sAngle);
}
else if(fIndex == 2)
{
double fOffset = 0;
fAng+=fOffset;
fAng = constrain(fAng, -pi/2, pi/2);
sAngle = mapFloat(fAng, -pi/2, pi/2, 500, 2350);
pwm.writeMicroseconds(fIndex, sAngle);
}
else if(fIndex == 3)
{
double fOffset = pi/4;
fAng+=fOffset;
fAng = constrain(fAng, -pi/2+pi/10, pi/2);
sAngle = mapFloat(fAng, -pi/2, pi/2, 500, 2350);
pwm.writeMicroseconds(fIndex, sAngle);
}
else if(fIndex == 1)
{
return;
}
else if(fIndex == 4)
{
double fOffset = 0;
fAng+=fOffset;
fAng = constrain(fAng, -pi/2+pi/10, pi/2);
sAngle = mapFloat(fAng, -pi/2, pi/2, 600, 2400);
pwm.writeMicroseconds(fIndex, sAngle);
}
}
volatile int flag1 = 0, flag2 = 0;
volatile int calsens1 = 0, calsens2 = 0;
void IRAM_ATTR finder1() {
calsens1 = encoder.getCount();
flag1 = 1;
}
void IRAM_ATTR finder2() {
calsens2 = encoder.getCount();
flag2 = 1;
}
void drive(int s1, int s2)
{
s1 = constrain(s1, -255, 255);
s2 = constrain(s2, -255, 255);
if(s1 > 0)
{
analogWrite(m1a, s1);
analogWrite(m1b, 0);
}
else
{
analogWrite(m1a, 0);
analogWrite(m1b, abs(s1));
}
if(s2 > 0)
{
analogWrite(m2a, s2);
analogWrite(m2b, 0);
}
else
{
analogWrite(m2a, 0);
analogWrite(m2b, abs(s2));
}
}
void motorMove(int speed, int brake)
{
if(speed == 0)
{
if(brake == 1)
{
digitalWrite(33,HIGH);
digitalWrite(32,HIGH);
}
else
{
digitalWrite(33,LOW);
digitalWrite(32,LOW);
}
analogWrite(13, 0);
return;
}
else if(speed > 0)
{
digitalWrite(33,LOW);
digitalWrite(32,HIGH);
}
else
{
digitalWrite(33,HIGH);
digitalWrite(32,LOW);
}
speed = abs(speed);
speed = constrain(speed, 0, 255);
analogWrite(13, speed);
}
const int closeEnough = 100;
const int farAway = 1000;
const int fullSpeed = 150;
const int minSpeed = 120;
int pos = 0;
int flag = 0;
void speedCurve(double target)
{
int encTarget = (int)mapFloat(target, -180, 180, -fullTurn/2, fullTurn/2);
int currPos = encoder.getCount()%fullTurn;
int delta = encTarget - currPos;
if(delta > fullTurn/2)
{
delta -= fullTurn;
}
else if(delta < (-fullTurn)/2)
{
delta+=fullTurn;
}
int mod = 1;
if(delta < 0)
{
mod = (-1);
}
if(abs(delta) <= closeEnough)
{
motorMove(0, 1);
}
else if(abs(delta) > farAway)
{
motorMove(fullSpeed*mod, 0);
}
else
{
motorMove((minSpeed + abs(delta)/farAway*(fullSpeed-minSpeed))*mod, 0);
}
}
void setup() {
pinMode(33, OUTPUT);
pinMode(32, OUTPUT);
pinMode(25, INPUT_PULLUP);
pinMode(26, INPUT_PULLUP);
pinMode(27, INPUT);
pinMode(14, INPUT);
pinMode(13, OUTPUT);
pinMode(m1a, OUTPUT);
pinMode(m1b, OUTPUT);
pinMode(m2a, OUTPUT);
pinMode(m2b, OUTPUT);
Serial.begin(115200);
digitalWrite(33,LOW);
digitalWrite(32,LOW);
analogWrite(13, 0);
frontLed.begin();
rearLed.begin();
for(int i = 0; i < NUMPIXELS; i++)
{
frontLed.setPixelColor(i, frontLed.Color(100,100,100));
rearLed.setPixelColor(i, frontLed.Color(100,0,0));
}
frontLed.show();
rearLed.show();
PS4.begin("70:08:94:2b:5f:b2");
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(50);
pwm.writeMicroseconds(0,angles[0]);
pwm.writeMicroseconds(1,angles[1]);
pwm.writeMicroseconds(2,angles[2]);
angleServo(3, 0);
pwm.writeMicroseconds(4,angles[4]);
pwm.writeMicroseconds(5,angles[5]);
ESP32Encoder::useInternalWeakPullResistors = puType::up;
encoder.attachHalfQuad(25, 26); // A, B
encoder.clearCount();
}
void loop() {
if(PS4.isConnected())
{
if(firstpass == 0)
{
firstpass = 1;
if(mode == 2)
{
PS4.setLed(255, 0, 0);
PS4.setFlashRate(0, 0);
for(int i = 0; i < NUMPIXELS; i++)
{
if(i < 2 || i >= 6)
{
frontLed.setPixelColor(i, frontLed.Color(25,25,25));
}
else
{
frontLed.setPixelColor(i, frontLed.Color(25,0,0));
}
rearLed.setPixelColor(i, frontLed.Color(50,0,0));
}
}
else if(mode == 1)
{
PS4.setLed(0, 0, 255);
PS4.setFlashRate(0, 0);
for(int i = 0; i < NUMPIXELS; i++)
{
if(i < 2 || i >= 6)
{
frontLed.setPixelColor(i, frontLed.Color(25,25,25));
}
else
{
frontLed.setPixelColor(i, frontLed.Color(0,0,25));
}
rearLed.setPixelColor(i, frontLed.Color(0,0,100));
}
}
frontLed.show();
rearLed.show();
PS4.sendToController();
delay(100);
}
if(PS4.L1()&&PS4.R1())
{
if(modeSwitchHold == 0)
{
modeSwitchHold = 1;
modeSwitchStart = millis();
PS4.setRumble(100, 100);
PS4.setLed(255, 255, 255);
PS4.setFlashRate(20, 20);
PS4.sendToController();
delay(10);
frontLed.clear();
rearLed.clear();
frontLed.show();
rearLed.show();
}
if(modeSwitched == 0)
{
int ledswipe = ((float)(millis()-modeSwitchStart))/100.0-1;
ledswipe = constrain(ledswipe, 0, 7);
frontLed.setPixelColor(ledswipe, frontLed.Color(50,50,50));
rearLed.setPixelColor(ledswipe, frontLed.Color(50,50,50));
frontLed.show();
rearLed.show();
}
if(millis()-modeSwitchStart>1000&&modeSwitched == 0)
{
PS4.setRumble(0, 0);
modeSwitched = 1;
if(mode == 1)
{
PS4.setLed(255, 0, 0);
PS4.setFlashRate(0, 0);
mode = 2;
for(int i = 0; i < NUMPIXELS; i++)
{
if(i < 2 || i >= 6)
{
frontLed.setPixelColor(i, frontLed.Color(25,25,25));
}
else
{
frontLed.setPixelColor(i, frontLed.Color(25,0,0));
}
rearLed.setPixelColor(i, frontLed.Color(50,0,0));
}
}
else if(mode == 2)
{
PS4.setLed(0, 0, 255);
PS4.setFlashRate(0, 0);
mode = 1;
for(int i = 0; i < NUMPIXELS; i++)
{
if(i < 2 || i >= 6)
{
frontLed.setPixelColor(i, frontLed.Color(25,25,25));
}
else
{
frontLed.setPixelColor(i, frontLed.Color(0,0,25));
}
rearLed.setPixelColor(i, frontLed.Color(0,0,100));
}
}
frontLed.show();
rearLed.show();
PS4.sendToController();
delay(500);
}
}
else
{
if(modeSwitchHold== 1 && modeSwitched == 0)
{
PS4.setRumble(0, 0);
PS4.setFlashRate(0, 0);
if(mode == 2)
{
PS4.setLed(255, 0, 0);
for(int i = 0; i < NUMPIXELS; i++)
{
if(i < 2 || i >= 6)
{
frontLed.setPixelColor(i, frontLed.Color(25,25,25));
}
else
{
frontLed.setPixelColor(i, frontLed.Color(25,0,0));
}
rearLed.setPixelColor(i, frontLed.Color(0,0,100));
}
frontLed.show();
rearLed.show();
}
else if(mode == 1)
{
PS4.setLed(0, 0, 255);
for(int i = 0; i < NUMPIXELS; i++)
{
if(i < 2 || i >= 6)
{
frontLed.setPixelColor(i, frontLed.Color(25,25,25));
}
else
{
frontLed.setPixelColor(i, frontLed.Color(0,0,25));
}
rearLed.setPixelColor(i, frontLed.Color(0,0,100));
}
frontLed.show();
rearLed.show();
}
PS4.sendToController();
delay(10);
}
modeSwitchHold = 0;
modeSwitched = 0;
PS4.setRumble(0, 0);
PS4.sendToController();
delay(10);
}
if(mode == 1)//DRIVING
{
int forward = constrain(PS4.R2Value()-PS4.L2Value(), -255, 255);
int sideways = map(PS4.LStickX(), -128, 128, -255, 255);
int rotate = mapFloat(PS4.RStickX(), -128, 128, -255, 255);
motorMove(rotate, 0);
simpleAngles[0]=map(clampFloat(encoder.getCount(), fullTurn/2), -fullTurn/2, fullTurn/2, -180, 180);
if(forward*forward+sideways*sideways<30*30)
{
drive(0,0);
}
else if(abs(sideways)>30)
{
if(forward > -20)
{
drive(forward+sideways, forward-sideways);
}
else
{
drive(forward-sideways, forward+sideways);
}
}
else
{
drive(forward, forward);
}
if(PS4.L3()&&PS4.L3())
{
frontLed.clear();
frontLed.setPixelColor(3, frontLed.Color(100,100,0));
frontLed.setPixelColor(4, frontLed.Color(100,100,0));
frontLed.show();
delay(100);
if(L3R3Timer == 0)
{
L3R3Timer = millis();
}
else if (millis() - L3R3Timer >= 1000)
{
Serial.println("Starting calibration!");
L3R3Timer = millis();
flag1 = 0;
flag2 = 0;
L3R3Timer = millis();
attachInterrupt(endPin2, finder2, RISING);
motorMove(180, 0);
while(flag2 == 0)
{
if(millis()-L3R3Timer>3000)
{
Serial.println("Calibration Failure S2");
break;
}
}
detachInterrupt(endPin2);
attachInterrupt(endPin1, finder1, RISING);
motorMove(0,0);
motorMove(-180, 0);
while(flag1 == 0)
{
if(millis()-L3R3Timer>3000)
{
Serial.println("Calibration Failure S1");
break;
}
}
detachInterrupt(endPin1);
motorMove(0,0);
delay(300);
Serial.println("Exiting calibration");
L3R3Timer = millis();
encoder.setCount(encoder.getCount()-((calsens2+calsens1)/2));
Serial.print("Found two sensors at: ");
Serial.print(calsens1);
Serial.print(", ");
Serial.println(calsens2);
Serial.println("Finished calibration!");
}
}
}
else if(mode == 2)//ARM CONTROLS
{
int rotate = mapFloat(PS4.R2Value()-PS4.L2Value(), -255, 255, -255, 255);
motorMove(rotate, 0);
simpleAngles[0]=map(clampFloat(encoder.getCount(), fullTurn/2), -fullTurn/2, fullTurn/2, -180, 180);
double move3 = dead(mapFloat(PS4.RStickY(), -127, 127, -0.01, 0.01), 0.002);
simpleAngles[2]+=move3;
angleServo(0, simpleAngles[2]);
double move2 = dead(mapFloat(PS4.RStickX(), -127, 127, -0.01, 0.01), 0.002);
simpleAngles[1]+=move2;
angleServo(5, simpleAngles[1]);
if(PS4.Left())
{
simpleAngles[3]-=0.02;
}
if(PS4.Right())
{
simpleAngles[3]+=0.02;
}
angleServo(2, simpleAngles[3]);
if(PS4.Up())
{
simpleAngles[4]+=0.02;
}
if(PS4.Down())
{
simpleAngles[4]-=0.02;
}
angleServo(3, simpleAngles[4]);
double move4 = dead(mapFloat(PS4.LStickX(), -127, 127, -0.02, 0.02), 0.002);
simpleAngles[5]+=move4;
angleServo(4, simpleAngles[5]);
if(PS4.Triangle()&&millis()-lastGrabTime>500)
{
if(hasGrabbed ==0)
{
hasGrabbed =1;
pwm.writeMicroseconds(1, 1340);
}
else
{
pwm.writeMicroseconds(1, 2000);
hasGrabbed =0;
}
lastGrabTime = millis();
}
if(PS4.Square())
{
simpleAngles[3] = 0;
simpleAngles[4] = 0;
simpleAngles[5] = 0;
angleServo(4, simpleAngles[5]);
angleServo(3, simpleAngles[4]);
angleServo(2, simpleAngles[3]);
}
}
}
else
{
firstpass = 0;
motorMove(0,0);
rainbow();
delay(20);
}
}
