{"id":619,"date":"2021-01-11T09:17:48","date_gmt":"2021-01-11T09:17:48","guid":{"rendered":"https:\/\/jan.decoster.studio\/?p=619"},"modified":"2021-05-11T08:43:21","modified_gmt":"2021-05-11T08:43:21","slug":"max-the-spot-micro-dog","status":"publish","type":"post","link":"https:\/\/jan.decoster.studio\/?p=619","title":{"rendered":"Max the robot dog"},"content":{"rendered":"\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Meet Max, the spot micro dog\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/w0mLF-rrgPk?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<p>Max is based on the Spot Micro project. <br>The Spot Micro is an open source project that is derived from the Spot Mini by Boston Dynamics. This robot is an example for a student assignment. The challenge was to make the robot walk with 12 identical servo&#8217;s and an Arduino Uno, within a few weeks of classes. The servo&#8217;s I used have 11 kg cm torque.  If you have the budget, 20 or 30 kg cm would give you more spare torque. <br>I used a standard RC 4ch controller to give input.  In this iteration the robot uses simple <a href=\"https:\/\/en.wikipedia.org\/wiki\/Forward_kinematics\" target=\"_blank\" rel=\"noopener\">Forward Kinematics<\/a> and a 2 step cycle. It&#8217;s comparable to 4 pogo sticks, and shifting balance to change direction. This is not how the real Spot Mini walks. For a smoother movement we need <a href=\"https:\/\/en.wikipedia.org\/wiki\/Inverse_kinematics\" target=\"_blank\" rel=\"noopener\">Inverse Kinematics<\/a>, so that we can calculate the position of each leg in real time. <\/p>\n\n\n\n<figure class=\"wp-block-image size-large is-resized\"><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9124-1024x683.jpg\" alt=\"\" class=\"wp-image-623\" width=\"512\" height=\"342\" srcset=\"https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9124-1024x683.jpg 1024w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9124-300x200.jpg 300w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9124-768x512.jpg 768w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9124-200x133.jpg 200w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9124.jpg 1404w\" sizes=\"auto, (max-width: 512px) 100vw, 512px\" \/><\/figure>\n\n\n\n<p><br>I have based the 3D prints on the design of Deok-yeon Kim. <a href=\"https:\/\/www.thingiverse.com\/kdy0523\/ \" target=\"_blank\" rel=\"noopener\">https:\/\/www.thingiverse.com\/kdy0523\/ <\/a><\/p>\n\n\n\n<p>The ears ant tail are my own additions.  The black parts are EVA foam. The yellow part of the ears are polystyrene.<br>The tip and base of the tail are 3D prints. You can download the STL files and PDF on the link below.<br><a rel=\"noreferrer noopener\" href=\"https:\/\/www.thingiverse.com\/thing:4855556\" target=\"_blank\">https:\/\/www.thingiverse.com\/thing:4855556<\/a><\/p>\n\n\n\n<p><br>Music: Monkey Warhol &#8211; Times of our life. Used under Creative Commons license. <br><br>This project was part of the course Next Media Maker held at Karel De Grote Hogeschool Antwerpen.<\/p>\n\n\n\n<figure class=\"wp-block-image size-large is-resized\"><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9161-1-1024x682.jpg\" alt=\"\" class=\"wp-image-624\" width=\"512\" height=\"341\" srcset=\"https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9161-1-1024x682.jpg 1024w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9161-1-300x200.jpg 300w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9161-1-768x512.jpg 768w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9161-1-200x133.jpg 200w, https:\/\/jan.decoster.studio\/wp-content\/uploads\/2021\/01\/editIMG_9161-1.jpg 1498w\" sizes=\"auto, (max-width: 512px) 100vw, 512px\" \/><\/figure>\n\n\n\n<p>I have converted my online classes into a series of video&#8217;s. (in Dutch) you can try automatic captions, but I can&#8217;t guarantee it won&#8217;t be very confusing. \ud83d\ude09 <\/p>\n\n\n\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Spot Micro tutorial Deel 1\/6 : Begin eraan !\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/OWNry3rdycI?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Spot Micro tutorial Deel 2\/6 : Assemblage\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/xJuJAnOn5oY?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Spot Micro tutorial Deel 3\/6 : Calibratie\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/i4Uqfibnn08?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Spot Micro tutorial Deel 4\/6 : Kinematics\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/OeENl2BWVf4?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Spot Micro tutorial Deel 5\/6 : Scripting\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/dCKS5rd_zag?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<figure class=\"wp-block-embed is-type-video is-provider-youtube wp-block-embed-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\n<iframe loading=\"lazy\" title=\"Spot Micro tutorial Deel 6\/6 : Tuning\" width=\"500\" height=\"281\" src=\"https:\/\/www.youtube.com\/embed\/vcVwlIBt8TM?feature=oembed\" frameborder=\"0\" allow=\"accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share\" referrerpolicy=\"strict-origin-when-cross-origin\" allowfullscreen><\/iframe>\n<\/div><\/figure>\n\n\n\n<h3 class=\"wp-block-heading\">Try out example code below <br><\/h3>\n\n\n\n<p class=\"has-white-color has-vivid-red-background-color has-text-color has-background\">Only of you have very thorough understanding of your robot, and you went trough calibration ! Code provided as is. Watch the online course before proceeding. (available soon)<\/p>\n\n\n<pre class=\"wp-block-code plover-prism prism-github-dark-theme language-clike\"><code>\/*\nSpot Robot controller\nJan De Coster\n07\/01\/2021\n\nMore info on www.jandecoster.com\nThis code is part of an online course. Using this code on your robot requires proper knowledge of the platform.\nDo not forget to calibrate your robot to avoid catastrophic failure ;) \n\nRC read based on \nKelvin Nelson's example 24\/07\/2019\nhttps:\/\/create.arduino.cc\/projecthub\/kelvineyeone\/read-pwm-decode-rc-receiver-input-and-apply-fail-safe-6b90eb\n\n*\/\n\n#include &lt;Servo.h&gt;\n\n\/\/RC stuff\n\/\/RC controller pins\nconst int CH_0_PIN = A0;\nconst int CH_1_PIN = A1;\nconst int CH_2_PIN = A2;\nconst int CH_3_PIN = A3;\nconst int deadzone = 20;  \/\/center stick when close to center\n\nint ch_0;\nint ch_1;\nint ch_2;\nint ch_3;\n\/\/End RC stuff\n\n\/\/Gyro stuff\n#include &lt;Wire.h&gt;\nconst int MPU = 0x68; \/\/ MPU6050 I2C address\nfloat AccX, AccY, AccZ;\nfloat GyroX, GyroY, GyroZ;\nfloat accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;\nfloat roll, pitch, yaw, oldroll, oldpitch, oldyaw;\nfloat AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;\nfloat elapsedTime, currentTime, previousTime;\nint c = 0;\n\nbool safetymode = false;\n\/\/End Gyro stuff\n\n\n\nServo servo[12]; \/\/array of servo's\n\nconst int servo_pin[12] =  {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13};  \/\/arrays of servo pins\n\nconst int directions[12] = {1, -1,-1,  1 ,1,1,  -1 ,1,1, -1,-1,-1};  \/\/reflection correction for opposite sides of the robot\n\nconst int base[12] = {90 ,100 ,102 ,  90 ,88 ,97 ,  90.00 ,78 ,95 ,  95 ,108 ,100 }; \n\/\/callibration   these are the base values for each servo, that put Spot in a comfortable starting position\n\n\n\n\/* Servo's kan not keep their current position, so we store these in an array angleCurrent\n * The target of each servo is placed in angleTarget, and angleSpeed holds each change in angle for each servo, per function call\n * arcPos holds different poses , with values relative to the base values in array base\n *\/\n\nfloat angleCurrent[12] = {base[0], base[1], base[2], base[3], base[4], base[5], base[6], base[7], base[8], base[9], base[10], base[11]}; \/\/real-time arcs\nfloat angleTarget[12] = {base[0], base[1], base[2], base[3], base[4], base[5], base[6], base[7], base[8], base[9], base[10], base[11]}; \/\/expected coordinates of the end of the leg\nfloat angleSpeed[12] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; \/\/each axis' speed, needs to be recalculated before each movement\n\n\/\/stap sequentie vooruit\nint sb1 = -10;  int so1 = 22;    \/\/sb upperarm  so lowerarm\nint sb2 = 2;    int so2 = 1;\nint sb3 = -9;   int so3 = 1;      \/\/only for 4 step moves\nint sb4 = -21;  int so4 = 19;\n\n\/\/intertia compensation after running\nint brb = 1;  int bro = -12;\n\n\/\/brace animation when stopping, to prevent tipping over\nint brcb = -14;  int brco = 24;\n\nint pootb = 53;  int pooto = -16;\n\n\n\n\n\nfloat arcPos[14][12] = {   \/\/2D array met standen voor elke servo, pas initialisatie aan wanneer je stappen toevoegt !\n        \/\/Leg Left Front------Leg right Front-----Leg Left rear----Leg Left rear    \n        \/\/ 0    \/\/base stand\n          {0,      0,     0,     0,     0,     0,     0,     0,     0,     0,      0,    0},   \n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n           \n        \/\/ 1    \/\/lay down, safety position when RC is lost\n          {0,     0,    45,     0,     0,    45,     0,     0,     45,    0,      0,   45}, \n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n           \n        \/\/ 2    \/\/stand back , rease back\n          {0,     0,    45,     0,     0,    45,     0,      0,     0,    0,      0,     0},\n      \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n          \n        \/\/ 3    \/\/sit up\n          {0,     0,    -15,      0,     0,   -15,     0,      0,    25,    0,      0,   25},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n\n        \/\/ 4    \/\/step 1   \n          {0,    sb1,    so1,     0,   sb2,   so2,   0,    sb1,   so1,    0,    sb2,   so2},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n        \n        \/\/ 5    \/\/step 2    \n          {0,    sb2,    so2,    0,   sb1,  so1,     0,    sb2,   so2,    0,      sb1,   so1},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n        \n         \/\/ 6    \/\/step 3\n          {0,    sb3,    so3,    0,   sb1,  so1,    0,    sb3,   so3,    0,    sb1 ,   so1  },\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n         \/\/ 7    \/\/step 4\n          {0,    sb4,    so4,   0,     sb2,    so2,   0,   sb3,   so3,    0,    sb2,     so2},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n         \/\/ 8    \/\/stand turn\n          {0,    0,     0,      0,      0,    0,        0,   0,   0,      0,    0,     0},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n        \n         \/\/ 9    \/\/break forward\n          {0,    0,     0,      0,      0,    0,        0,   sb1\/3,   so1\/3,      0,    sb1\/3,   so1\/3},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n        \/\/ 10    \/\/break backward\n          {0,    0,  0,   0,   0,  0,   0,   brb,   bro,      0,    brb,  bro },\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n        \/\/ 11    \/\/brace \n          {0,    brcb,  brco,   0,   brcb,  brco,   0,   brcb,   brco,      0,  brcb,  brco},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n        \n          \/\/ 12    \/\/sniff \n          {0,    brcb,  brco,   0,   brcb,  brco,   0,   0,   0,      0,  0,  0},\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n\n         \/\/ 13    \/\/raise \n          {0,    0,  0,   0,   0,  0,   0,   brcb,   brco,      0,  brcb,  brco}\n        \/\/shldr   up    down  shldr  up    down     shldr  up    down    shldr  up    down  \n          };\n\n        \nbool zitten = false; \/\/is Spot sitting down?\nbool moving = false; \/\/is Spot moving? then we should wait\nbool staan = false; \/\/is Spot standing?\nbool movingforward = false; \/\/is Spot moving forward?\nbool movingbackward = false; \/\/is Spot moving backward?\nbool jumped = false; \/\/did Spot jump?\nbool sniffing = false; \/\/is Spot sniffing?\nbool raised = false; \/\/is Spot looking up?\n\nint draaihoek = 0;  \/\/left right\nint tilthoek = 0;   \/\/ up down\nint fspeed = 0;     \/\/speed\n\nint currentstep = 1;\n\nvoid setup() {\n  Serial.begin(9600); \n\n  Wire.begin();                      \/\/ Initialize comunication\n  Wire.beginTransmission(MPU);       \/\/ Start communication with MPU6050 \/\/ MPU=0x68\n  Wire.write(0x6B);                  \/\/ Talk to the register 6B\n  Wire.write(0x00);                  \/\/ Make reset - place a 0 into the 6B register\n  Wire.endTransmission(true);        \/\/end the transmission\n  Serial.println(\"Gyro\");\n  calculate_IMU_error();\n  delay(20);\n  \n  Serial.println(\"relay\");\n  \n  \/\/pinMode(1, OUTPUT); \/\/1 when pin available connect this to a relay for extra safety\n  \/\/digitalWrite(1, LOW);\n  delay(500);\n  Serial.println(\"attach\");\n  servo_attach();\n\n  setTargets(1,10); \/\/go to lay down, without filling target arrays\n  \n  \/\/digitalWrite(1, HIGH); \/\/start motors with relay\n  delay(1000);\n  Serial.println(\"set\");\n\n  \n  setTargets(1,10); \/\/go to lay down\n  zitten = true;\n\n  delay(1000);\n \n  \n\n}\n\nvoid loop() {\n\ncheckRC();  \/\/read RC\n\nreadGyro();\n\nif(!safetymode){\n\ndraaihoek = ch_0\/40;\nfspeed = -ch_1\/25;\ntilthoek = ch_3\/20;\nforwardarray();\nturnarray();\n\n  \/\/walking? switch step 1 and 2\n  if(abs(ch_1)&gt;40 &amp;&amp; !zitten &amp;&amp; currentstep==1){\n      if(ch_1&gt;0){\n        movingforward = true;\n        }else{\n          movingbackward = true;\n          }\n    currentstep = 2;\n    setTargets(4,10);\n    } \n  else if(abs(ch_1)&gt;40 &amp;&amp; !zitten &amp;&amp; currentstep==2){\n     if(ch_1&gt;0){\n        movingforward = true;\n        }else{\n           movingbackward = true;\n          }\n    currentstep = 1;\n    setTargets(5,10);\n    }\n  \/\/not walking ?  \n  else{\n  \n        if(ch_2&gt;0){                 \/\/gaat channel 2 boven 0?\n          if(zitten){               \/\/Spot sits? stand\n            anim_lig();\n            anim_standup();\n            zitten = false;\n            }\n            else{                     \/\/Spot not sitting? go to stand\n              \n              zitten = false;\n                 \n              if(abs(draaihoek)&gt;2 &amp;&amp; !zitten){\n                \n                \n                setTargets(8,10);\n                }else{\n                  if(movingforward){\n                    setTargets(9,5);\n                    movingforward = false;\n                    }\n                  else if(movingbackward){\n                    setTargets(10,5);\n                    movingbackward = false;\n                    }\n                  anim_stand();\n                  }\n                  \n              } \n          }\n        \n        if(ch_2&lt;-100 &amp;&amp; !zitten){     \/\/gaat channel 2 onder -100?\n          zitten = true;\n          anim_zit();                 \/\/ Spot sit\n          }\n          \n        else if(ch_2&lt;-200 &amp;&amp; zitten){      \/\/gaat channel 2 onder -200?\n          anim_lig();                 \/\/Spot lay down\n          }\n\n        else if(ch_2&gt;220 &amp;&amp; !jumped){      \/\/gaat channel 2 boven 200?\n          anim_jump();                 \/\/Spot jump\n          jumped = true;\n          } \n        else if(ch_2&lt;200){\n          jumped = false;\n          }  \n        if(ch_3&gt;220 &amp;&amp; !sniffing &amp;&amp; !zitten){\n         anim_sniff();    \n         sniffing = true;\n          }   \n        else if(ch_3&lt;200){\n         sniffing = false;\n          }\n        if(ch_3&lt;-200 &amp;&amp; !raised &amp;&amp; !zitten){\n         anim_raise();    \n         raised = true;\n          }   \n        else if(ch_3&gt;-180){\n         raised = false;\n          }      \n          \n        \n    }\n}\n\nelse if(ch_2&lt;-200){  \/\/leave safety by going to sitting\n  zitten= true;\n  safetymode = false;\n  \n  }\n}\n\nvoid forwardarray(){ \/\/adjust angles accoridng to input\nint comp = -draaihoek*0.9;\n  \nif(fspeed&gt;0){  \/\/\nsb1 = -10-abs(fspeed\/1.5);  so1 = 22-abs(fspeed\/1.5);\nsb2 = 2-abs(fspeed\/1.5);    so2 = 1-abs(fspeed\/1.5);\n}else{      \/\/achteruit niet te snel\nsb1 = -10-abs(fspeed\/3);  so1 = 22-abs(fspeed\/3);\nsb2 = 2-abs(fspeed\/3);    so2 = 1-abs(fspeed\/3);\n  }\n\n  arcPos[4][1] = sb1-draaihoek;         arcPos[4][2] = so1-draaihoek-comp;                  arcPos[4][4] = sb2+fspeed+draaihoek;  arcPos[4][5] = so2+draaihoek+comp+fspeed\/2;   arcPos[4][7] = sb1+draaihoek;          arcPos[4][8] = so1+draaihoek+comp;             arcPos[4][10] = sb2+fspeed-draaihoek; arcPos[4][11] = so2+fspeed\/2-draaihoek+comp; \n  arcPos[5][1] = sb2+fspeed-draaihoek;  arcPos[5][2] = so2+fspeed\/2-draaihoek-comp;         arcPos[5][4] = sb1+draaihoek;         arcPos[5][5] = so1+draaihoek+comp;            arcPos[5][7] = sb2+fspeed+draaihoek;   arcPos[5][8] = so2+fspeed\/2+draaihoek+comp;    arcPos[5][10] = sb1-draaihoek;        arcPos[5][11] = so1-draaihoek+comp; \n}\n\nvoid turnarray(){  \/\/adjust angles accoridng to input\n\n  int comp = -draaihoek*0.9;\n\n \n  arcPos[4][0] = 0;      arcPos[4][3] = -draaihoek;     arcPos[4][6] = 0;           arcPos[4][9] = draaihoek; \n  arcPos[5][0] = -draaihoek;      arcPos[5][3] = 0;     arcPos[5][6] = draaihoek;   arcPos[5][9] = 0; \n  \n  arcPos[8][0] = -draaihoek; \n  arcPos[8][1] = -draaihoek;      \n  arcPos[8][2] = -draaihoek-comp;       \n  arcPos[8][3] = -draaihoek;\n  arcPos[8][4] = draaihoek;       \n  arcPos[8][5] = draaihoek+comp;         \n  arcPos[8][6] = draaihoek;\n  arcPos[8][7] = draaihoek;  \n  arcPos[8][8] = draaihoek+comp;          \n  arcPos[8][9] = draaihoek;\n  arcPos[8][10] = -draaihoek; \n  arcPos[8][11] = -draaihoek-comp;  \n  }\n\n\nvoid anim_standup(){  \/\/sit up, back first, then front legs\n   setTargets(2,5);\n   setTargets(0,8);\n  }\n\nvoid anim_lig(){   \/\/lay down\n  setTargets(1,8);\n  }\n  \nvoid anim_zit(){ \/\/sit\n  setTargets(3,2);\n  }\n\nvoid anim_stand(){ \/\/go to stand not from sitting\n   setTargets(0,2);\n  }\n\nvoid anim_trappel(){ \/\/go to stand not from sitting\n   setTargets(4,15);\n   setTargets(5,15);\n  }\n\nvoid anim_jump(){\n  setTargets(0,2);\n  setTargets(11,4);\n  setTargets(0,20);\n  }\n\nvoid anim_sniff(){\n  setTargets(12,2);\n  }  \n  \nvoid anim_raise(){\n  setTargets(13,2);\n  }  \n\n        \nvoid setTargets(int pos, float myspeed){    \/\/ pos = row in array    myspeed = speed from 1 to 20\n\nmoving = true;\nfloat delta[12];\nfloat maxDelta = 0;\n\nif(myspeed&gt;20){ myspeed = 20;}\nif(myspeed&lt;1){ myspeed = 1;}\n\nfor (int i=0; i &lt; 12; i++){\n \n  angleTarget[i] = base[i]+arcPos[pos][i]*directions[i];\n  delta[i] = base[i]+arcPos[pos][i]*directions[i] - angleCurrent[i]; \n  \n  if(abs(delta[i])&gt;maxDelta){\n    maxDelta = abs(delta[i]);\n    }\n  \n  if(delta[i]&gt;0){\n    angleSpeed[i] = myspeed;\n    }  \n  else if(delta[i]&lt;0){\n    angleSpeed[i] = -myspeed;\n    } \n  else{\n      angleSpeed[i] = 0;\n      }  \n \n  } \n \n  while(moving){  \/\/repeat function while servo's not at destination\n    setServos();\n    delay(5); \/\/give it some time\n    }\n \n}\n\nvoid setServos(){\n  \n  int speedcount = 0; \/\/count all servo's that are still moving\n\n  \/\/for each servo, what is the distance between current angle and target angle\n  \/\/smaller then 1? make current angle target angle en put speed to 0\n  \/\/if not, add speed to current angle with factor 1\/10 adjusted with speed\n\n  \n  for (int i=0; i &lt; 12; i++){\n    if(abs(angleTarget[i]-angleCurrent[i])&lt;=1){\n        angleCurrent[i] = angleTarget[i];\n        angleSpeed[i] = 0;\n        }else{\n         angleCurrent[i] += angleSpeed[i]\/10; \n          }\n  \n    servo[i].write(constrain(angleCurrent[i],10,170));  \/\/angles are put in the array, now put the servo at right position\n  }\n\n  \/\/are servo's still moving?\n  for (int i=0; i &lt; 12; i++){\n    speedcount += abs(angleSpeed[i]);\n    }\n  \/\/if not  (speedcount = 0) stop moving, while loop stops\n  if(speedcount==0){\n    moving = false;\n    }  \n    \n}\n\n\nvoid servo_attach(void){  \n  for (int i = 0; i &lt; 12; i++)\n  {\n    servo[i].attach(servo_pin[i]);\n    \n    delay(100);\n  }\n}\n\n\nvoid checkRC(){ \/\/convert RC pulses to variables\n   \/\/ Read pulse width from receiver\n  ch_0 = pulseIn(CH_0_PIN, HIGH, 25000);\n  ch_1 = pulseIn(CH_1_PIN, HIGH, 25000);\n  ch_2 = pulseIn(CH_2_PIN, HIGH, 25000);\n  ch_3 = pulseIn(CH_3_PIN, HIGH, 25000);\n\n  ch_0 = pulseToPWM(ch_0);\n  ch_1 = pulseToPWM(ch_1);\n  ch_2 = pulseToPWM(ch_2);\n  ch_3 = pulseToPWM(ch_3);\n  \n\n  }\n\n\n\/\/ Convert RC pulse value to motor PWM value\nint pulseToPWM(int pulse) {\n  \n  \/\/ pulses between 1000 and 2000 converted to -255 and 255\n  \n  if ( pulse &gt; 1000 ) {\n    pulse = map(pulse, 1000, 2000, -500, 500);\n    pulse = constrain(pulse, -255, 255);\n  } else {\n    pulse = 0;\n  }\n\n  \/\/ Anything in deadzone should stop the motor\n  if ( abs(pulse) &lt;= deadzone ) {\n    pulse = 0;\n  }\n\n  return pulse;\n}\n\n\n\nvoid readAcc(){\n  \/\/ === Read acceleromter data === \/\/\n  Wire.beginTransmission(MPU);\n  Wire.write(0x3B); \/\/ Start with register 0x3B (ACCEL_XOUT_H)\n  Wire.endTransmission(false);\n  Wire.requestFrom(MPU, 6, true); \/\/ Read 6 registers total, each axis value is stored in 2 registers\n  \/\/For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet\n  AccX = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 16384.0; \/\/ X-axis value\n  AccY = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 16384.0; \/\/ Y-axis value\n  AccZ = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 16384.0; \/\/ Z-axis value\n  \/\/ Calculating Roll and Pitch from the accelerometer data\n  accAngleX = (atan(AccY \/ sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 \/ PI) - 0.58; \/\/ AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details\n  accAngleY = (atan(-1 * AccX \/ sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 \/ PI) + 1.58; \/\/ AccErrorY ~(-1.58)\n  \n  }\n  \nvoid readGyro(){\n\/\/ === Read gyroscope data === \/\/\n  previousTime = currentTime;        \/\/ Previous time is stored before the actual time read\n  currentTime = millis();            \/\/ Current time actual time read\n  elapsedTime = (currentTime - previousTime) \/ 1000; \/\/ Divide by 1000 to get seconds\n  Wire.beginTransmission(MPU);\n  Wire.write(0x43); \/\/ Gyro data first register address 0x43\n  Wire.endTransmission(false);\n  Wire.requestFrom(MPU, 6, true); \/\/ Read 4 registers total, each axis value is stored in 2 registers\n  GyroX = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 131.0; \/\/ For a 250deg\/s range we have to divide first the raw value by 131.0, according to the datasheet\n  GyroY = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 131.0;\n  GyroZ = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 131.0;\n  \/\/ Correct the outputs with the calculated error values\n  GyroX = GyroX + 0.56; \/\/ GyroErrorX ~(-0.56)\n  GyroY = GyroY - 2; \/\/ GyroErrorY ~(2)\n  GyroZ = GyroZ + 0.79; \/\/ GyroErrorZ ~ (-0.8)\n\n  \/\/ Currently the raw values are in degrees per seconds, deg\/s, so we need to multiply by sendonds (s) to get the angle in degrees\n  gyroAngleX = gyroAngleX + GyroX * elapsedTime; \/\/ deg\/s * s = deg\n  gyroAngleY = gyroAngleY + GyroY * elapsedTime;\n  yaw =  yaw + GyroZ * elapsedTime;\n\n  \/\/ Complementary filter - combine acceleromter and gyro angle values\n  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;\n  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;\n  \n\/\/  \/\/ Print the values on the serial monitor\n\/\/  Serial.print(roll);\n\/\/  Serial.print(\"\/\");\n\/\/  Serial.print(pitch);\n\/\/  Serial.print(\"\/\");\n\/\/  Serial.println(yaw);\n\n    if(abs(GyroX)&gt;200){\n      Serial.println(GyroX);\n      safetymode = true;\n      setTargets(1,5);\n      \n      }\n  if(abs(GyroY)&gt;200){\n      Serial.println(GyroY);\n      safetymode = true;\n      setTargets(1,5);\n      }\n    if(abs(GyroZ)&gt;200){\n      Serial.println(GyroZ);\n      safetymode = true;\n      setTargets(1,5);\n      }  \n}\n\nvoid calculate_IMU_error() {\n  \/\/ We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.\n  \/\/ Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values\n  \/\/ Read accelerometer values 200 times\n  while (c &lt; 200) {\n    Wire.beginTransmission(MPU);\n    Wire.write(0x3B);\n    Wire.endTransmission(false);\n    Wire.requestFrom(MPU, 6, true);\n    AccX = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 16384.0 ;\n    AccY = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 16384.0 ;\n    AccZ = (Wire.read() &lt;&lt; 8 | Wire.read()) \/ 16384.0 ;\n    \/\/ Sum all readings\n    AccErrorX = AccErrorX + ((atan((AccY) \/ sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 \/ PI));\n    AccErrorY = AccErrorY + ((atan(-1 * (AccX) \/ sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 \/ PI));\n    c++;\n  }\n  \/\/Divide the sum by 200 to get the error value\n  AccErrorX = AccErrorX \/ 200;\n  AccErrorY = AccErrorY \/ 200;\n  c = 0;\n  \/\/ Read gyro values 200 times\n  while (c &lt; 200) {\n    Wire.beginTransmission(MPU);\n    Wire.write(0x43);\n    Wire.endTransmission(false);\n    Wire.requestFrom(MPU, 6, true);\n    GyroX = Wire.read() &lt;&lt; 8 | Wire.read();\n    GyroY = Wire.read() &lt;&lt; 8 | Wire.read();\n    GyroZ = Wire.read() &lt;&lt; 8 | Wire.read();\n    \/\/ Sum all readings\n    GyroErrorX = GyroErrorX + (GyroX \/ 131.0);\n    GyroErrorY = GyroErrorY + (GyroY \/ 131.0);\n    GyroErrorZ = GyroErrorZ + (GyroZ \/ 131.0);\n    c++;\n  }\n  \/\/Divide the sum by 200 to get the error value\n  GyroErrorX = GyroErrorX \/ 200;\n  GyroErrorY = GyroErrorY \/ 200;\n  GyroErrorZ = GyroErrorZ \/ 200;\n  \/\/ Print the error values on the Serial Monitor\n  Serial.print(\"AccErrorX: \");\n  Serial.println(AccErrorX);\n  Serial.print(\"AccErrorY: \");\n  Serial.println(AccErrorY);\n  Serial.print(\"GyroErrorX: \");\n  Serial.println(GyroErrorX);\n  Serial.print(\"GyroErrorY: \");\n  Serial.println(GyroErrorY);\n  Serial.print(\"GyroErrorZ: \");\n  Serial.println(GyroErrorZ);\n}<\/code><\/pre>\n\n\n","protected":false},"excerpt":{"rendered":"<p>Max is based on the Spot Micro project. <\/p>\n","protected":false},"author":1,"featured_media":627,"comment_status":"closed","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[2],"tags":[],"class_list":["post-619","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-robots"],"_links":{"self":[{"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/posts\/619","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=619"}],"version-history":[{"count":15,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/posts\/619\/revisions"}],"predecessor-version":[{"id":722,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/posts\/619\/revisions\/722"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=\/wp\/v2\/media\/627"}],"wp:attachment":[{"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=619"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=619"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/jan.decoster.studio\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=619"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}