Hardware components | ||||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
Software apps and online services | ||||||
| ||||||
![]() |
|
In the Rover Challenge the vehicles need to be equipped with a digital compass designed for the competition. The team from Germany (International Space Education Institute, Leipzig, Germany - High School Division), composed of several students need to create a digital compass, where they select a course so that when they reach that part of the circuit they apply it correctly.
In this project only the software part is shown, and with the pieces of the kit the students have created a structure to test this prototype.
The project is easy. On the one hand it shows the value returned by the compass (heading) which is also corrected with the magnetic declination angle, which can be searched on the Internet indicating the place where the compass will be located.
The user inputs the angle in relation to North (0°) by using the KY-040 and locks or unlocks it by pressing it. Whether the angle is locked in gets displayed (red = locked; green = unlocked). This angle including the orientation of the instrument itself gets displayed by the arrow (cyan) on the screen. In addition to that the direction of North gets displayed by a purple line. The input-value is also displayed in the top row to give the user a “live-feedback” of what he is doing.
Magnetometer HMC5883L
Magnetometer HMC5883L measures the direction and magnitude of the Earth’s magnetic field and hence is used for low cost compassing and magnetometry.
It measures the Earth’s magnetic field value along the X, Y and Z axes from milli-gauss to 8 gauss.
It can be used as a compass to find direction or to find the direction of heading of a device.
It makes use of I2C protocol for communication with a microcontroller. Earth’s magnetic field values along the X, Y and Z axes can be found by reading values from addresses of certain registers using I2C communication.
The X axe in our project will be aligned with the north in our display
Take care: Because this is a magnetic compass if you put it near to battery, metallic surface or magnetic field, the result will change from the actual. Also make sure to use it on a flat surface.
This is a tool to create arduino code using graphical blocks. It is ready for MRT-duino board but you can change the configuration to works with other arduino boards. You can find information about the pins and ports of the board here.
My students do many projects using this tool because it allows them to learn computer thinking without getting into the knowledge of a specific programming language.
Information about the libraries to install depending of the sensor or actuator used in the proyect, click here.
In this case these libraries were used:
- Adafruit GFX: to use the TFT7735
- Adafruit ST7735: to use the TFT7735
- Encoder: to use the encoder sensor
- HMC5883 Adafruit: to use with the HMC5883 magnetometer-compass sensor
- MrtRemote: library to use two features: the MRT remote control using the dedicated connector and the start button of the board
The tool have a lot of sensors and actuators. Soon IOT projects using ESP-01 wifi chip will be published. The Pixy2 camera is integrated too, and soon I hope to integrate the Tello dron. The target is the students can manage a lot of components :-)
ConnectionThe connection to the board is configure in the block. You can check the source code in this part:
Compass
- SDA to Port 13. S pin of this port
- SCL to Port 14. S pin of this port
- VCC to 5V. + pin of the any port
- Ground to GND. - pins of the any port
TFT ST7735
- SCK to Port 2. S pin of this port
- SDA to Port 3. S pin of this port
- CS to Port 5. S pin of this port
- A0 to Port 6. S pin of this port
- RST to Port 7. S pin of this port
- Led to 3.3V. in the black arduino leonardo pin of this board. Black pins
- VCC to 5V. + pin of the any port
- Ground to GND. - pins of the any port
Rotary Encoder
- DT to Port 5. S pin of this port
- CLK to Port 16. S pin of this port
- Button SW to Port 1. S pin of this port
- VCC to 5V. + pin of the any port
- Ground to GND. - pins of the any port
<xml xmlns="http://www.w3.org/1999/xhtml"><block type="arduino_setup" x="-87" y="12"><statement name="MySetup"><block type="variables_declare"><field name="VAR">Fixed</field><field name="TYPE">int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Declination</field><field name="TYPE">float</field><value name="VALUE"><block type="math_number"><field name="NUM">0.12</field></block></value><next><block type="variables_declare"><field name="VAR">HeadingOK</field><field name="TYPE">float</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastHeading</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Angle</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastAngle</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Course</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">radius</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">60</field></block></value><next><block type="variables_declare"><field name="VAR">X0</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">64</field></block></value><next><block type="variables_declare"><field name="VAR">Y0</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">80</field></block></value><next><block type="variables_declare"><field name="VAR">X1</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Y1</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastX1</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastY1</field><field name="TYPE">unsigned int</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Xc</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Yc</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastXc</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastYc</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">X2</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Y2</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastX2</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastY2</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">X3</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastX3</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">Y3</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">LastY3</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="variables_declare"><field name="VAR">db</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">5</field></block></value><next><block type="variables_declare"><field name="VAR">dl</field><field name="TYPE">long</field><value name="VALUE"><block type="math_number"><field name="NUM">10</field></block></value><next><block type="Init_RotaryEncoderInterrupt"><field name="RE_NUMBER">1</field><field name="PINDT">0</field><field name="PINCLK">1</field><next><block type="RotaryEncoder_Write"><field name="RE_NUMBER">1</field><value name="Writecounter"><shadow type="math_number"><field name="NUM">0</field></shadow></value><next><block type="serial_init"><field name="PINBAUDIOS">115200</field><next><block type="Init_Compass_HMC5883"><next><block type="st7735_init"><field name="PIN_CS">18</field><field name="PIN_DC">19</field><field name="PIN_RST">20</field><field name="WRAP">INITR_BLACKTAB</field><next><block type="procedures_callnoreturn"><mutation name="Presentation"></mutation><next><block type="base_delays"><value name="DELAY_TIME"><shadow type="math_number"><field name="NUM">1</field></shadow><block type="math_number"><field name="NUM">7</field></block></value><next><block type="procedures_callnoreturn"><mutation name="DisplayCircleAndMarkNorth"></mutation></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></statement><statement name="MyLoop"><block type="procedures_callnoreturn"><mutation name="CalculateNorth"></mutation><next><block type="procedures_callnoreturn"><mutation name="SetAngel"></mutation><next><block type="procedures_callnoreturn"><mutation name="DisplayAxes"></mutation><next><block type="controls_if"><value name="IF0"><block type="internal_button_sensor"></block></value><statement name="DO0"><block type="procedures_callnoreturn"><mutation name="Help"></mutation><next><block type="base_delays"><value name="DELAY_TIME"><shadow type="math_number"><field name="NUM">1</field></shadow><block type="math_number"><field name="NUM">7</field></block></value><next><block type="procedures_callnoreturn"><mutation name="DisplayCircleAndMarkNorth"></mutation><next><block type="procedures_callnoreturn"><mutation name="DisplayAngleCourse"></mutation><next><block type="controls_if"><mutation else="1"></mutation><value name="IF0"><block type="logic_compare"><field name="OP">EQ</field><value name="A"><block type="variables_get"><field name="VAR">Fixed</field></block></value><value name="B"><block type="math_number"><field name="NUM">0</field></block></value></block></value><statement name="DO0"><block type="procedures_callnoreturn"><mutation name="DisplayUnlock"></mutation></block></statement><statement name="ELSE"><block type="procedures_callnoreturn"><mutation name="DisplayLock"></mutation></block></statement><next><block type="variables_set"><field name="VAR">LastHeading</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value></block></next></block></next></block></next></block></next></block></next></block></statement><next><block type="tempo_no_delay"><field name="unit">ms</field><value name="DELAY_TIME"><shadow type="math_number"><field name="NUM">5</field></shadow><block type="math_number"><field name="NUM">500</field></block></value><statement name="branch"><block type="procedures_callnoreturn"><mutation name="logs"></mutation></block></statement></block></next></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1062" y="62"><field name="NAME">CalculateNorth</field><statement name="STACK"><block type="order_to_read_HMC5883_values"><next><block type="variables_set"><field name="VAR">HeadingOK</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="HMC5883_values"><field name="TypeMag">3</field></block></value><value name="B"><block type="variables_get"><field name="VAR">Declination</field></block></value></block></value><next><block type="controls_if"><value name="IF0"><block type="logic_compare"><field name="OP">LT</field><value name="A"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value><value name="B"><block type="math_number"><field name="NUM">0</field></block></value></block></value><statement name="DO0"><block type="variables_set"><field name="VAR">HeadingOK</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></statement></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1712" y="63"><field name="NAME">logs</field><statement name="STACK"><block type="serial_print"><value name="CONTENT"><shadow type="text"><field name="TEXT">text</field></shadow><block type="text_join"><mutation items="3"></mutation><value name="ADD0"><block type="text"><field name="TEXT">Heading with declination applied: </field></block></value><value name="ADD1"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value><value name="ADD2"><block type="text"><field name="TEXT"></field></block></value></block></value><next><block type="serial_print"><value name="CONTENT"><shadow type="text"><field name="TEXT">text</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="text"><field name="TEXT">Fixed: </field></block></value><value name="ADD1"><block type="variables_get"><field name="VAR">Fixed</field></block></value></block></value><next><block type="serial_print"><value name="CONTENT"><shadow type="text"><field name="TEXT">text</field></shadow><block type="text_join"><mutation items="3"></mutation><value name="ADD0"><block type="text"><field name="TEXT">Angle: </field></block></value><value name="ADD1"><block type="variables_get"><field name="VAR">Angle</field></block></value><value name="ADD2"><block type="text"><field name="TEXT"></field></block></value></block></value></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1062" y="363"><field name="NAME">SetAngel</field><statement name="STACK"><block type="controls_if"><value name="IF0"><block type="logic_compare"><field name="OP">EQ</field><value name="A"><block type="variables_get"><field name="VAR">Fixed</field></block></value><value name="B"><block type="math_number"><field name="NUM">0</field></block></value></block></value><statement name="DO0"><block type="variables_set"><field name="VAR">Angle</field><value name="VALUE"><block type="RotaryEncoder_Read"><field name="RE_NUMBER">1</field></block></value><next><block type="controls_if"><value name="IF0"><block type="logic_operation"><field name="OP">OR</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">Angle</field></block></value><value name="B"><block type="math_number"><field name="NUM">359</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LT</field><value name="A"><block type="variables_get"><field name="VAR">Angle</field></block></value><value name="B"><block type="math_number"><field name="NUM">0</field></block></value></block></value></block></value><statement name="DO0"><block type="variables_set"><field name="VAR">Angle</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="RotaryEncoder_Write"><field name="RE_NUMBER">1</field><value name="Writecounter"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value></block></next></block></statement><next><block type="controls_if"><value name="IF0"><block type="logic_compare"><field name="OP">NEQ</field><value name="A"><block type="variables_get"><field name="VAR">Angle</field></block></value><value name="B"><block type="variables_get"><field name="VAR">LastAngle</field></block></value></block></value><statement name="DO0"><block type="variables_set"><field name="VAR">LastAngle</field><value name="VALUE"><block type="variables_get"><field name="VAR">Angle</field></block></value><next><block type="procedures_callnoreturn"><mutation name="DisplayAngleCourse"></mutation></block></next></block></statement></block></next></block></next></block></statement><next><block type="controls_if"><value name="IF0"><block type="rotaryencoder_button_sensor"><field name="RE_NUMBER">1</field><field name="PIN_RE_BUTTON">13</field></block></value><statement name="DO0"><block type="controls_if"><mutation else="1"></mutation><value name="IF0"><block type="logic_compare"><field name="OP">EQ</field><value name="A"><block type="variables_get"><field name="VAR">Fixed</field></block></value><value name="B"><block type="math_number"><field name="NUM">0</field></block></value></block></value><statement name="DO0"><block type="variables_set"><field name="VAR">Fixed</field><value name="VALUE"><block type="math_number"><field name="NUM">1</field></block></value><next><block type="procedures_callnoreturn"><mutation name="DisplayLock"></mutation></block></next></block></statement><statement name="ELSE"><block type="variables_set"><field name="VAR">Fixed</field><value name="VALUE"><block type="math_number"><field name="NUM">0</field></block></value><next><block type="RotaryEncoder_Write"><field name="RE_NUMBER">1</field><value name="Writecounter"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Angle</field></block></value><next><block type="procedures_callnoreturn"><mutation name="DisplayUnlock"></mutation></block></next></block></next></block></statement><next><block type="base_delayms"><value name="DELAY_TIME"><shadow type="math_number"><field name="NUM">1000</field></shadow><block type="math_number"><field name="NUM">900</field></block></value></block></next></block></statement></block></next></block></statement></block><block type="procedures_defnoreturn" x="1887" y="587"><field name="NAME">DisplayAngleCourse</field><statement name="STACK"><block type="st7735_drawrectangle"><field name="COLOR">ST7735_BLACK</field><field name="LOGIC">TRUE</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">55</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><value name="width"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="math_number"><field name="NUM">65</field></block></value><value name="height"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="math_number"><field name="NUM">15</field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_WHITE</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">55</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">Angle</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> degrees</field></block></value></block></value></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1888" y="863"><field name="NAME">Help</field><statement name="STACK"><block type="st7735_backgroundcolor"><field name="COLOR">ST7735_BLACK</field><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_WHITE</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">50</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">HELP</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT"></field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">-You can select the angle of the course with the encoder</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT"></field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">-With button encoder you fix the angle of the course.Press again to unlock</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT"></field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">-White mark is the North</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT"></field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_MAGENTA</field><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">- Magenta line is heading measures with compass</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT"></field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_CYAN</field><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">- Cyan line is the angle of your course</field></block></value></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1063" y="1163"><field name="NAME">DisplayLock</field><statement name="STACK"><block type="st7735_settextcolor"><field name="COLOR">ST7735_BLACK</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Unlock</field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_RED</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Lock</field></block></value></block></next></block></next></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1238" y="1538"><field name="NAME">DisplayUnlock</field><statement name="STACK"><block type="st7735_settextcolor"><field name="COLOR">ST7735_BLACK</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Lock</field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_GREEN</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Unlock</field></block></value></block></next></block></next></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="-87" y="1637"><field name="NAME">DisplayCircleAndMarkNorth</field><statement name="STACK"><block type="st7735_backgroundcolor"><field name="COLOR">ST7735_BLACK</field><next><block type="st7735_settextsize"><field name="SIZE">1</field><next><block type="st7735_drawcircle"><field name="COLOR">ST7735_WHITE</field><field name="LOGIC">FALSE</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="radius"><shadow type="math_number"><field name="NUM">50</field></shadow><block type="variables_get"><field name="VAR">radius</field></block></value><next><block type="st7735_drawline"><field name="COLOR">ST7735_WHITE</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="B"><block type="variables_get"><field name="VAR">radius</field></block></value></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="B"><block type="variables_get"><field name="VAR">radius</field></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">10</field></block></value></block></value><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_GREEN</field><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Unlock</field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_WHITE</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">55</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">150</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">0 degrees</field></block></value></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="1913" y="1637"><field name="NAME">Presentation</field><statement name="STACK"><block type="st7735_backgroundcolor"><field name="COLOR">ST7735_BLACK</field><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_RED</field><next><block type="st7735_settextsize"><field name="SIZE">2</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">25</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">10</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">DIGITAL</field></block></value><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">25</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">40</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">COMPASS</field></block></value><next><block type="st7735_settextsize"><field name="SIZE">1</field><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_WHITE</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">25</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">85</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Roverchallenge</field></block></value><next><block type="st7735_settextsize"><field name="SIZE">2</field><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_CYAN</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">50</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">140</field></block></value><next><block type="st7735_printTextln"><field name="LOGIC">TRUE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text"><field name="TEXT">Logix5</field></block></value></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></statement></block><block type="procedures_defnoreturn" x="-67" y="2090"><field name="NAME">DisplayAxes</field><statement name="STACK"><block type="controls_if"><value name="IF0"><block type="logic_compare"><field name="OP">NEQ</field><value name="A"><block type="conversion_toint"><value name="NAME"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value></block></value><value name="B"><block type="variables_get"><field name="VAR">LastHeading</field></block></value></block></value><statement name="DO0"><block type="st7735_drawline"><field name="COLOR">ST7735_BLACK</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">LastX1</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">LastY1</field></block></value><next><block type="variables_set"><field name="VAR">LastHeading</field><value name="VALUE"><block type="conversion_toint"><value name="NAME"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value></block></value><next><block type="variables_set"><field name="VAR">X1</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">X0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">SIN</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="variables_set"><field name="VAR">Y1</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">COS</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="st7735_drawline"><field name="COLOR">ST7735_MAGENTA</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">X1</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">Y1</field></block></value><next><block type="variables_set"><field name="VAR">LastX1</field><value name="VALUE"><block type="variables_get"><field name="VAR">X1</field></block></value><next><block type="variables_set"><field name="VAR">LastY1</field><value name="VALUE"><block type="variables_get"><field name="VAR">Y1</field></block></value><next><block type="variables_set"><field name="VAR">Course</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">HeadingOK</field></block></value><value name="B"><block type="variables_get"><field name="VAR">Angle</field></block></value></block></value><next><block type="controls_if"><value name="IF0"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="math_number"><field name="NUM">359</field></block></value></block></value><statement name="DO0"><block type="variables_set"><field name="VAR">Course</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></statement><next><block type="st7735_drawline"><field name="COLOR">ST7735_BLACK</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">LastXc</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">LastYc</field></block></value><next><block type="st7735_drawtriangle"><field name="COLOR">ST7735_BLACK</field><field name="LOGIC">TRUE</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">LastXc</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">LastYc</field></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">LastX2</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">10</field></shadow><block type="variables_get"><field name="VAR">LastY2</field></block></value><value name="x2"><shadow type="math_number"><field name="NUM">10</field></shadow><block type="variables_get"><field name="VAR">LastX3</field></block></value><value name="y2"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">LastY3</field></block></value><next><block type="variables_set"><field name="VAR">Xc</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">X0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">SIN</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="variables_set"><field name="VAR">Yc</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">COS</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="variables_set"><field name="VAR">X2</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">X0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="variables_get"><field name="VAR">dl</field></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">SIN</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="variables_get"><field name="VAR">db</field></block></value></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="variables_set"><field name="VAR">Y2</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="variables_get"><field name="VAR">dl</field></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">COS</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="variables_get"><field name="VAR">db</field></block></value></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="variables_set"><field name="VAR">X3</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">ADD</field><value name="A"><block type="variables_get"><field name="VAR">X0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="variables_get"><field name="VAR">dl</field></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">SIN</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="variables_get"><field name="VAR">db</field></block></value></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="variables_set"><field name="VAR">Y3</field><value name="VALUE"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">radius</field></block></value><value name="B"><block type="variables_get"><field name="VAR">dl</field></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">5</field></block></value></block></value><value name="B"><block type="math_single"><field name="OP">COS</field><value name="NUM"><block type="math_arithmetic"><field name="OP">DIVIDE</field><value name="A"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_arithmetic"><field name="OP">MINUS</field><value name="A"><block type="variables_get"><field name="VAR">Course</field></block></value><value name="B"><block type="variables_get"><field name="VAR">db</field></block></value></block></value><value name="B"><block type="math_arithmetic"><field name="OP">MULTIPLY</field><value name="A"><block type="math_number"><field name="NUM">2</field></block></value><value name="B"><block type="math_constant"><field name="CONSTANT">PI</field></block></value></block></value></block></value><value name="B"><block type="math_number"><field name="NUM">360</field></block></value></block></value></block></value></block></value></block></value><next><block type="st7735_drawline"><field name="COLOR">ST7735_CYAN</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">X0</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Y0</field></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">Xc</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">Yc</field></block></value><next><block type="st7735_drawtriangle"><field name="COLOR">ST7735_CYAN</field><field name="LOGIC">TRUE</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Xc</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="variables_get"><field name="VAR">Yc</field></block></value><value name="x1"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">X2</field></block></value><value name="y1"><shadow type="math_number"><field name="NUM">10</field></shadow><block type="variables_get"><field name="VAR">Y2</field></block></value><value name="x2"><shadow type="math_number"><field name="NUM">10</field></shadow><block type="variables_get"><field name="VAR">X3</field></block></value><value name="y2"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="variables_get"><field name="VAR">Y3</field></block></value><next><block type="variables_set"><field name="VAR">LastXc</field><value name="VALUE"><block type="variables_get"><field name="VAR">Xc</field></block></value><next><block type="variables_set"><field name="VAR">LastYc</field><value name="VALUE"><block type="variables_get"><field name="VAR">Yc</field></block></value><next><block type="variables_set"><field name="VAR">LastX2</field><value name="VALUE"><block type="variables_get"><field name="VAR">X2</field></block></value><next><block type="variables_set"><field name="VAR">LastY2</field><value name="VALUE"><block type="variables_get"><field name="VAR">Y2</field></block></value><next><block type="variables_set"><field name="VAR">LastX3</field><value name="VALUE"><block type="variables_get"><field name="VAR">X3</field></block></value><next><block type="variables_set"><field name="VAR">LastY3</field><value name="VALUE"><block type="variables_get"><field name="VAR">Y3</field></block></value><next><block type="procedures_callnoreturn"><mutation name="DisplayHeadingWithText"></mutation></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></statement></block></statement></block><block type="procedures_defnoreturn" x="1112" y="2087"><field name="NAME">DisplayHeadingWithText</field><statement name="STACK"><block type="st7735_drawrectangle"><field name="COLOR">ST7735_BLACK</field><field name="LOGIC">TRUE</field><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">80</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><value name="width"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="math_number"><field name="NUM">45</field></block></value><value name="height"><shadow type="math_number"><field name="NUM">100</field></shadow><block type="math_number"><field name="NUM">15</field></block></value><next><block type="st7735_settextcolor"><field name="COLOR">ST7735_WHITE</field><next><block type="st7735_setcursor"><value name="x0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">80</field></block></value><value name="y0"><shadow type="math_number"><field name="NUM">0</field></shadow><block type="math_number"><field name="NUM">0</field></block></value><next><block type="controls_if"><mutation elseif="7"></mutation><value name="IF0"><block type="logic_operation"><field name="OP">OR</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">350</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">10</field></block></value></block></value></block></value><statement name="DO0"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> N</field></block></value></block></value></block></statement><value name="IF1"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">10</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">80</field></block></value></block></value></block></value><statement name="DO1"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> NE</field></block></value></block></value></block></statement><value name="IF2"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">80</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">100</field></block></value></block></value></block></value><statement name="DO2"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> E</field></block></value></block></value></block></statement><value name="IF3"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">100</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">170</field></block></value></block></value></block></value><statement name="DO3"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> SE</field></block></value></block></value></block></statement><value name="IF4"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">170</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">190</field></block></value></block></value></block></value><statement name="DO4"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> S</field></block></value></block></value></block></statement><value name="IF5"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">190</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">260</field></block></value></block></value></block></value><statement name="DO5"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> SW</field></block></value></block></value></block></statement><value name="IF6"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">260</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">290</field></block></value></block></value></block></value><statement name="DO6"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> W</field></block></value></block></value></block></statement><value name="IF7"><block type="logic_operation"><field name="OP">AND</field><value name="A"><block type="logic_compare"><field name="OP">GT</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">290</field></block></value></block></value><value name="B"><block type="logic_compare"><field name="OP">LTE</field><value name="A"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="B"><block type="math_number"><field name="NUM">350</field></block></value></block></value></block></value><statement name="DO7"><block type="st7735_printTextln"><field name="LOGIC">FALSE</field><value name="text_to_print"><shadow type="text"><field name="TEXT">I am a Robot</field></shadow><block type="text_join"><mutation items="2"></mutation><value name="ADD0"><block type="variables_get"><field name="VAR">LastHeading</field></block></value><value name="ADD1"><block type="text"><field name="TEXT"> NW</field></block></value></block></value></block></statement></block></next></block></next></block></next></block></statement></block></xml>
/*******************************************
*** ARDUINO CODE -- MRTDUINO BOARD ***
*** Logix5 -- www.logix5.com ***
********************************************/
#include <Encoder.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <MrtDuino.h>
int Fixed;
float Declination;
float HeadingOK;
unsigned int LastHeading;
unsigned int Angle;
unsigned int LastAngle;
unsigned int Course;
unsigned int radius;
unsigned int X0;
unsigned int Y0;
unsigned int X1;
unsigned int Y1;
unsigned int LastX1;
unsigned int LastY1;
long Xc;
long Yc;
long LastXc;
long LastYc;
long X2;
long Y2;
long LastX2;
long LastY2;
long X3;
long LastX3;
long Y3;
long LastY3;
long db;
long dl;
Encoder encoder_1(0,1);
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
sensors_event_t eventmag;
Adafruit_ST7735 tft1=Adafruit_ST7735(18,19,20);
MrtIdSetting ir_id_set;
long tempms500 = 0 ;
void CalculateNorth() {
mag.getEvent(&eventmag);
HeadingOK = (atan2(eventmag.magnetic.y, eventmag.magnetic.x)*(180/M_PI)) - Declination;
if (HeadingOK < 0) {
HeadingOK = HeadingOK + 360;
}
}
void logs() {
Serial.println((String("Heading with declination applied: ") + String(HeadingOK) + String("")));
Serial.println((String("Fixed: ") + String(Fixed)));
Serial.println((String("Angle: ") + String(Angle) + String("")));
}
void SetAngel() {
if (Fixed == 0) {
Angle = (encoder_1.read()/4);
if (Angle > 359 || Angle < 0) {
Angle = 0;
encoder_1.write(0*4);
}
if (Angle != LastAngle) {
LastAngle = Angle;
DisplayAngleCourse();
}
}
if (!digitalRead(13)) {
if (Fixed == 0) {
Fixed = 1;
DisplayLock();
} else {
Fixed = 0;
encoder_1.write(Angle*4);
DisplayUnlock();
}
delay(900);
}
}
void DisplayAngleCourse() {
tft1.fillRect(55,150,65,15,ST7735_BLACK);
tft1.setTextColor(ST7735_WHITE);
tft1.setCursor(55,150);
tft1.print((String(Angle) + String(" degrees")));
}
void Help() {
tft1.fillScreen(ST7735_BLACK);
tft1.setTextColor(ST7735_WHITE);
tft1.setCursor(50,0);
tft1.println("HELP");
tft1.println("");
tft1.println("-You can select the angle of the course with the encoder");
tft1.println("");
tft1.println("-With button encoder you fix the angle of the course.Press again to unlock");
tft1.println("");
tft1.println("-White mark is the North");
tft1.println("");
tft1.setTextColor(ST7735_MAGENTA);
tft1.println("- Magenta line is heading measures with compass");
tft1.println("");
tft1.setTextColor(ST7735_CYAN);
tft1.println("- Cyan line is the angle of your course");
}
void DisplayLock() {
tft1.setTextColor(ST7735_BLACK);
tft1.setCursor(0,150);
tft1.print("Unlock");
tft1.setTextColor(ST7735_RED);
tft1.setCursor(0,150);
tft1.print("Lock");
}
void DisplayUnlock() {
tft1.setTextColor(ST7735_BLACK);
tft1.setCursor(0,150);
tft1.print("Lock");
tft1.setTextColor(ST7735_GREEN);
tft1.setCursor(0,150);
tft1.print("Unlock");
}
void DisplayCircleAndMarkNorth() {
tft1.fillScreen(ST7735_BLACK);
tft1.setTextSize(1);
tft1.drawCircle(X0,Y0,radius,ST7735_WHITE);
tft1.drawLine(X0,(Y0 - radius),X0,((Y0 - radius) - 10),ST7735_WHITE);
tft1.setCursor(0,150);
tft1.setTextColor(ST7735_GREEN);
tft1.print("Unlock");
tft1.setTextColor(ST7735_WHITE);
tft1.setCursor(55,150);
tft1.print("0 degrees");
}
void Presentation() {
tft1.fillScreen(ST7735_BLACK);
tft1.setTextColor(ST7735_RED);
tft1.setTextSize(2);
tft1.setCursor(25,10);
tft1.println("DIGITAL");
tft1.setCursor(25,40);
tft1.println("COMPASS");
tft1.setTextSize(1);
tft1.setTextColor(ST7735_WHITE);
tft1.setCursor(25,85);
tft1.println("Roverchallenge");
tft1.setTextSize(2);
tft1.setTextColor(ST7735_CYAN);
tft1.setCursor(50,140);
tft1.println("Logix5");
}
void DisplayAxes() {
if (int(HeadingOK) != LastHeading) {
tft1.drawLine(X0,Y0,LastX1,LastY1,ST7735_BLACK);
LastHeading = int(HeadingOK);
X1 = X0 + (radius - 5) * sin(((HeadingOK * (2 * PI)) / 360));
Y1 = Y0 - (radius - 5) * cos(((HeadingOK * (2 * PI)) / 360));
tft1.drawLine(X0,Y0,X1,Y1,ST7735_MAGENTA);
LastX1 = X1;
LastY1 = Y1;
Course = HeadingOK + Angle;
if (Course > 359) {
Course = Course - 360;
}
tft1.drawLine(X0,Y0,LastXc,LastYc,ST7735_BLACK);
tft1.fillTriangle(LastXc,LastYc,LastX2,LastY2,LastX3,LastY3,ST7735_BLACK);
Xc = X0 + (radius - 5) * sin(((Course * (2 * PI)) / 360));
Yc = Y0 - (radius - 5) * cos(((Course * (2 * PI)) / 360));
X2 = X0 + ((radius - dl) - 5) * sin((((Course + db) * (2 * PI)) / 360));
Y2 = Y0 - ((radius - dl) - 5) * cos((((Course + db) * (2 * PI)) / 360));
X3 = X0 + ((radius - dl) - 5) * sin((((Course - db) * (2 * PI)) / 360));
Y3 = Y0 - ((radius - dl) - 5) * cos((((Course - db) * (2 * PI)) / 360));
tft1.drawLine(X0,Y0,Xc,Yc,ST7735_CYAN);
tft1.fillTriangle(Xc,Yc,X2,Y2,X3,Y3,ST7735_CYAN);
LastXc = Xc;
LastYc = Yc;
LastX2 = X2;
LastY2 = Y2;
LastX3 = X3;
LastY3 = Y3;
DisplayHeadingWithText();
}
}
void DisplayHeadingWithText() {
tft1.fillRect(80,0,45,15,ST7735_BLACK);
tft1.setTextColor(ST7735_WHITE);
tft1.setCursor(80,0);
if (LastHeading > 350 || LastHeading <= 10) {
tft1.print((String(LastHeading) + String(" N")));
} else if (LastHeading > 10 && LastHeading <= 80) {
tft1.print((String(LastHeading) + String(" NE")));
} else if (LastHeading > 80 && LastHeading <= 100) {
tft1.print((String(LastHeading) + String(" E")));
} else if (LastHeading > 100 && LastHeading <= 170) {
tft1.print((String(LastHeading) + String(" SE")));
} else if (LastHeading > 170 && LastHeading <= 190) {
tft1.print((String(LastHeading) + String(" S")));
} else if (LastHeading > 190 && LastHeading <= 260) {
tft1.print((String(LastHeading) + String(" SW")));
} else if (LastHeading > 260 && LastHeading <= 290) {
tft1.print((String(LastHeading) + String(" W")));
} else if (LastHeading > 290 && LastHeading <= 350) {
tft1.print((String(LastHeading) + String(" NW")));
}
}
void setup()
{
Fixed = 0;
Declination = 0.12;
HeadingOK = 0;
LastHeading = 0;
Angle = 0;
LastAngle = 0;
Course = 0;
radius = 60;
X0 = 64;
Y0 = 80;
X1 = 0;
Y1 = 0;
LastX1 = 0;
LastY1 = 0;
Xc = 0;
Yc = 0;
LastXc = 0;
LastYc = 0;
X2 = 0;
Y2 = 0;
LastX2 = 0;
LastY2 = 0;
X3 = 0;
LastX3 = 0;
Y3 = 0;
LastY3 = 0;
db = 5;
dl = 10;
Serial.begin(115200);
mag.begin();
tft1.initR(INITR_BLACKTAB);
encoder_1.write(0*4);
Presentation();
delay((long)7*1000);
DisplayCircleAndMarkNorth();
pinMode(13,INPUT_PULLUP);
}
void loop()
{
CalculateNorth();
SetAngel();
DisplayAxes();
if (ir_id_set.isStartPressed()) {
Help();
delay((long)7*1000);
DisplayCircleAndMarkNorth();
DisplayAngleCourse();
if (Fixed == 0) {
DisplayUnlock();
} else {
DisplayLock();
}
LastHeading = 0;
}
if ((millis()-tempms500)>=500) {
tempms500=millis();
logs();
}
}
Comments