{"id":597,"date":"2014-08-11T10:25:55","date_gmt":"2014-08-11T10:25:55","guid":{"rendered":"http:\/\/www.dyalog.com\/blog\/?p=597"},"modified":"2014-08-11T10:25:55","modified_gmt":"2014-08-11T10:25:55","slug":"making-controlled-turns-with-the-dyabot","status":"publish","type":"post","link":"https:\/\/www.dyalog.com\/blog\/2014\/08\/making-controlled-turns-with-the-dyabot\/","title":{"rendered":"Making Controlled Turns with the DyaBot"},"content":{"rendered":"<p>This blog originally started\u00a0when I took delivery of the DyaBot, a Raspberry Pi and Arduino based variant of the <a title=\"C3Pi on YouTube\" href=\"https:\/\/www.youtube.com\/watch?v=OKwawXB4n_0\">C3Pi<\/a>\u00a0running Dyalog v13.2. The architecture of the &#8216;bot and instructions for building your own inexpensive robot\u00a0can all be found in blog entries from <a title=\"April Blog Entries\" href=\"http:\/\/www.dyalog.com\/blog\/2013\/04\/\">April<\/a>\u00a0to July of last year.<\/p>\n<p>The downside of only using inexpensive components is that some of them are not very precise. The worst problem we face is that the amount of wheel movement generated by the application of a particular power level varies from one motor to the next, and indeed from moment to moment. Trying to drive a specific distance in a straight line, or make an exact 90 degree turn regardless of the surface that the &#8216;bot is standing on, are impossible tasks with the original Dyabot. You can have a lot of fun, as I hope the early posts <a title=\"APL-Controlled Robot Performs Death-Defying Stunts Using PiCam\" href=\"http:\/\/www.dyalog.com\/blog\/2013\/09\/apl-controlled-robot-performs-death-defying-stunts-using-picam\/\">demonstrate<\/a>, but we have higher ambitions!<\/p>\n<p>Our next task is to add motion sensors to the DyaBot, with a goal of being\u00a0able to measure actual motion with sufficient accuracy to maintain our heading while driving straight ahead \u2013 and to make exact turns to new headings, like the 90-degree turn made in this video (the &#8216;bot has been placed on a Persian carpet to provide a background containing right angles):<\/p>\n<p><iframe loading=\"lazy\"  id=\"_ytid_60846\"  width=\"584\" height=\"329\"  data-origwidth=\"584\" data-origheight=\"329\"  src=\"https:\/\/www.youtube.com\/embed\/ff9WurJr6Ho?enablejsapi=1&#038;autoplay=0&#038;cc_load_policy=0&#038;iv_load_policy=1&#038;loop=0&#038;modestbranding=0&#038;rel=1&#038;fs=1&#038;playsinline=0&#038;autohide=2&#038;theme=dark&#038;color=red&#038;controls=1&#038;\" class=\"__youtube_prefs__  epyt-is-override  no-lazyload\" title=\"YouTube player\"  allow=\"autoplay; encrypted-media\" allowfullscreen data-no-lazy=\"1\" data-skipgform_ajax_framebjll=\"\"><\/iframe><\/p>\n<h2>Introducing the\u00a0MPU-6050<\/h2>\n<p>For some time we have had an <a title=\"MPU-6050\" href=\"http:\/\/www.invensense.com\/mems\/gyro\/mpu6050.html\" target=\"_blank\">MPU-6050<\/a>, which has 3-axis rotation and acceleration sensors,\u00a0attached to our I2C bus, but haven&#8217;t been able to use it. About ten days ago (sorry, I took a few days off last week), <a title=\"Romilly Cocking on Twitter\" href=\"https:\/\/twitter.com\/romillyc\" target=\"_blank\">@RomillyC<\/a> came to visit us in Bramley to help me read some <a title=\"Python MPU-6050 Example\" href=\"http:\/\/blog.bitify.co.uk\/search\/label\/MPU-6050\" target=\"_blank\">Python code<\/a> that was written for this device. The current acceleration or rotation on each axis is constantly available as a register and can be queried via I2C. Translated into APL, the code to read the current rate of rotation around the vertical axis is:<\/p>\n<pre><code class=\"language-apl\"> \u2207 r\u2190z_gyro_reading \u235d current z-axis rotation in degrees\r\n   r\u2190(read_word_2c 70)\u00f7131 \u235d Read register #70 and scale to deg\/sec\r\n \u2207\r\n\r\n \u2207 r\u2190read_word_2c n;zz\r\n   r\u2190I2 256\u22a5read_byte_2c\u00a8n+0 1               \u235d Read 2 registers and make signed I2\r\n \u2207\r\n\r\n \u2207 r\u2190read_byte_2c n;z\r\n   z\u2190#.I2C.WriteArray i2c_address (1,n)0     \u235d Write register address\r\n   r\u21902 2\u2283#.I2C.ReadArray i2c_address (1 0)1  \u235d Read input\r\n \u2207\r\n\r\n I2\u2190{\u2375&gt;32767:1-65535-\u2375 \u22c4 \u2375} \u235d 16-bit 2's complement to signed<\/code><\/pre>\n<h2>Integrating to Track Attitude<\/h2>\n<p>Reading the register directly gives us the instantaneous rate of rotation. If we want to track the attitude, we need to integrate the rates over time. The next layer of functions allows us to reset the attitude and perform very primitive integration by multiplying the current rotation with the elapsed time since the last measurement:<\/p>\n<pre><code class=\"language-apl\">\u2207 r\u2190z_gyro_reset            \u235d Reset Gyro Integration\r\n  z_gyro_time\u2190now           \u235d current time in ms\r\n  z_gyro_posn\u21900             \u235d current rotation is 0\r\n\u2207\r\n\r\n\u2207 r\u2190z_gyro;t;delta          \u235d Integrated gyro rotation\r\n  t\u2190now                     \u235d current time in ms\r\n  delta\u21900.001\u00d7t-z_gyro_time \u235d elapsed time in seconds\r\n  r\u2190z_gyro_posn\u2190z_gyro_posn+delta\u00d7z_gyro_reading+z_gyro_offset\r\n  z_gyro_time\u2190t\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 \u235d last time\r\n\u2207\r\n<\/code><\/pre>\n<p>We can now write a function to rotate through a given number of degrees (assuming that the gyro has just been reset): we set the wheels in motion, monitor the integrated angle until we are nearly there, then shut off the engines. We continue logging the angles for a brief moment to monitor the deceleration. The function returns a two-column matrix containing the &#8220;log&#8221; of rotation angles and timestamps:<\/p>\n<pre><code class=\"language-apl\">    \u2207 log\u2190rotate angle;bot;i;t\r\n     \r\n      log\u21900 2\u23740 \u235d Angle, time\r\n      bot.Speed\u21903 \u00af3 \u235d Slow counter-clockwise rotation\r\n     \r\n      :Trap 1000 \u235d Catch interrupts\r\n          :Repeat\r\n              log\u236a\u2190(t\u2190z_gyro),now\r\n          :Until t&gt;angle-7 \u235d Stop rotating 7 degrees before we are done    \r\n      :EndTrap\r\n\r\n      bot.Speed\u21900 \u235d cut power to the motors\r\n      :For i :In \u237325 \u235d Capture data as we slow down\r\n          log\u236a\u2190z_gyro,now\r\n      :EndFor\r\n    \u2207\r\n<\/code><\/pre>\n<p>The logged data from the rotation captured in the video at the top is charted below:<\/p>\n<p><a href=\"http:\/\/www.dyalog.com\/blog\/wp-content\/uploads\/2014\/08\/Controlled-90-Degree-Rotation.png\"><img loading=\"lazy\" decoding=\"async\" class=\"aligncenter size-full wp-image-599\" src=\"http:\/\/www.dyalog.com\/blog\/wp-content\/uploads\/2014\/08\/Controlled-90-Degree-Rotation.png\" alt=\"Controlled 90-Degree Rotation\" width=\"758\" height=\"542\" srcset=\"https:\/\/www.dyalog.com\/blog\/wp-content\/uploads\/2014\/08\/Controlled-90-Degree-Rotation.png 758w, https:\/\/www.dyalog.com\/blog\/wp-content\/uploads\/2014\/08\/Controlled-90-Degree-Rotation-300x214.png 300w, https:\/\/www.dyalog.com\/blog\/wp-content\/uploads\/2014\/08\/Controlled-90-Degree-Rotation-419x300.png 419w\" sizes=\"auto, (max-width: 758px) 100vw, 758px\" \/><\/a>Even with the primitive integration and rotation strategies, the results already look quite promising. I&#8217;ll be taking most of this week off \u2013 part of it without access to the internet(!), but once I am back, expect the next blog entry to explore writing functions that accelerate and slow down in a more controlled fashion as well as stop at the right spot rather than relying on a specific amount of friction to rotate through the last 7 degrees (note the very slight reverse rotation at the end, probably caused by the Persian carpet being a bit &#8220;springy&#8221;). I will also clean up the code and post the complete solution on GitHub \u2013 and perhaps even look at some better integration techniques.<\/p>\n<p>If you would like to make sure you don&#8217;t miss the next installment, follow the <a title=\"RSS Feed\" href=\"http:\/\/www.dyalog.com\/blog\/feed\/\" target=\"_blank\">RSS feed<\/a> or <a title=\"@DyalogAPL\" href=\"https:\/\/twitter.com\/dyalogapl\" target=\"_blank\">Dyalog on Twitter<\/a>.<\/p>\n","protected":false},"excerpt":{"rendered":"<p>This blog originally started\u00a0when I took delivery of the DyaBot, a Raspberry Pi and Arduino based variant of the C3Pi\u00a0running Dyalog v13.2. The architecture of the &#8216;bot and instructions for building your own inexpensive robot\u00a0can all be found in blog &hellip; <a href=\"https:\/\/www.dyalog.com\/blog\/2014\/08\/making-controlled-turns-with-the-dyabot\/\">Continue reading <span class=\"meta-nav\">&rarr;<\/span><\/a><\/p>\n","protected":false},"author":2,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[12,9,5],"tags":[],"class_list":["post-597","post","type-post","status-publish","format-standard","hentry","category-cto","category-raspberry-pi","category-robots"],"_links":{"self":[{"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/posts\/597","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/users\/2"}],"replies":[{"embeddable":true,"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/comments?post=597"}],"version-history":[{"count":32,"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/posts\/597\/revisions"}],"predecessor-version":[{"id":654,"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/posts\/597\/revisions\/654"}],"wp:attachment":[{"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/media?parent=597"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/categories?post=597"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.dyalog.com\/blog\/wp-json\/wp\/v2\/tags?post=597"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}