Published February 1, 2020

Digital Compass

In the Rover Challenge need a digital compass to have clear the orientation in the contest. Don't miss out

BeginnerFull instructions provided3 hours807

Things used in this project

Hardware components

Beduino kit

This is a educational kit. It was created in spain with pieces of My Robot Time Company. The board is an arduino Leonardo board with motor drivers

ST7735 TFT display

HMC5883 compass

Rotative encoder

dupont cables

Software apps and online services

Blocklyduino

Arduino IDE

Story

Target

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.

Digital compass with MRTduino Board

Blocklyduino and MRT-Duino board

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 :-)

Connection

The 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();
    }

}

Credits

OscarF10

4 projects • 4 followers

I am telecommunication engineer.In my free time I am doing things related of robotics,technology for students in our courses in logix5

Digital Compass

Things used in this project

Hardware components

Software apps and online services

Story

Target

Blocklyduino and MRT-Duino board

Connection

Code

DigitalCompass_with_help

DigitalComapss_with_help

Examples with Blocklyduino

Source code of Blocklyduino

Credits

OscarF10

Comments

Embed the widget on your own site

Digital Compass

Digital Compass

Things used in this project

Hardware components

Software apps and online services

Story

Target

Blocklyduino and MRT-Duino board

Connection

Code

DigitalCompass_with_help

DigitalComapss_with_help

Examples with Blocklyduino

Source code of Blocklyduino

Credits

OscarF10

Comments

Related channels and tags