Oh wait! Didn't I mention the toolchain to program the Seeeduino XIAO in Materials section in my previous blog?! How are we going to program the Seeeduino XIAO?
Well, the obvious choice would be to use Arduino IDE since it has all the tools that are needed to program the MCU. And there is huge community backing and libraries available to make things easier. However, I like to use something like Visual Studio Code since it has a lot of other features, such as intellisense, terminal and endless customisation. That's about environment but what about the OS/libraries?
Let's wind back a little and look at the overall picture how it (the sunray bender) will all work. There will be sensing to do, actuation to do, talking to do and perhaps some logging to do as well, all at the same time. And therefore, we will need something like a real-time operating system. Basic programs that has everything happening in a single loop will make things work one thing at a time. For example, if I run the code that is mentioned on this link, I will have the following result.
Only one motor moves at once! This is not what we want. We want to move both motors to move at the same time. Also, we want to be sensing at the same time, too. We can't do sense and then move and then sense again to see if we reached desired position. We need to sense continuously at the same time when the motors are moving, to regulate them accordingly and to know when to stop!
Now, back to where we were, I looked at the Getting started guide for Seeeduino XIAO which led me to using Seeeduino XIAO with Zephyr. Now, we are talking. It looked promising! So, I began installing everthing needed based on Zephyr OS project documentation. I first tried to set it up on my Raspberry Pi (model 4B) with Ubuntu. However, I had issues with it not recognising compiler properly and some python dependencies could not be installed! Never mind, I then used my desktop computer with Ubuntu on it. Success! I could build the sample project and flash it on my Arduino Nano 33 IOT (I used it to test the toolchain before moving on to Seeeduino XIAO) from terminal. However, I struggled to set up VS Code to use Zephyr. I followed a lot of tutorials, YouTube videos, went through all the types of Google searches and forums that I could possibly think of. But I couldn't get the intellisense to work. Those annoying squiggly lines kept appearing because it could't find the files from relevant include folders even when path was provided. I had never used Zephyr before so I wasn't prepared to use simple text editor to write the program without ability to debug, find definitions, etc. I then moved to my Windows machine. But yet again, different issues! Zephyr could build it but can't flash it with west flash
. Something called bossac
could not be found. I tried to use the one that was in Arduino's path but it didn't work either. I must have spent about 3 full days and some sleepless nights!
I had used FreeRTOS before and so I thought why not use the same again. I found that FreeRTOS (the original library) doesn't support SAMD21 boards but fortunately, someone (known as Scott Briscoe, thanks to you ) had written another library based on FreeRTOS that supports SAMD21 boards! So, I used good old Platform IO on VS Code and installed this library, and then built the sample code and flashed it on the Seeeduino XIAO. And hooray, it worked!
This means now I have a way to program the MCU to do what I want. So, to test the motors, I modified the sample program where I included the functions from sample program for DRI0017 in the FreeRTOS tasks to run both motors simultaneously. However, nothing moved! I discovered that the logic level might be an issue which was the case. I had believed that the L298 would be able to work with 3.3V logic but for some reason it didn't work. Not to worry, let's put some logic level shifter. But what now! It didn't work! I spent hours trying to troubleshoot, looked at the wiring, measured voltages, etc., changed to another logic level shifter circuit board, made a transistor switch (few resistors and BC547B) instead of using shifter board, but nothing seemed to get me anywhere. The level at HV side couldn't drive (as in provide 5V logic to) the motor driver circuit. In the end, I decided to use Arduino Uno to do the motor driving. I know that this defies the purpose of using Seeeduino XIAO for everything in the project but let's consider the Uno as a peripheral device.
Following is the code to run on the Seeeduino XIAO:
//************************************************************************** // FreeRtos on Samd21 // Template by Scott Briscoe // // Code editor: Hitesh Boghani // Testing motors for the Sunray bender //************************************************************************** #include <FreeRTOS_SAMD21.h> #include <Wire.h> //************************************************************************** // Type Defines and Constants //************************************************************************** #define ERROR_LED_PIN 13 //Led Pin: Typical Arduino Board #define MOTOR_AZIMUTH_SPD 0 // motor1 direction #define MOTOR_ELEVATION_SPD 1 // motor1 speed #define MOTOR_AZIMUTH_DIR 2 // motor2 direction #define MOTOR_ELEVATION_DIR 3 // motor2 speed #define I2C_SDA 4 // SDA pin for I2C #define I2C_SCL 5 // SCL pin for I2C #define ERROR_LED_LIGHTUP_STATE HIGH // the state that makes the led light up on your board, either low or high // Select the serial port the project should use and communicate over // Some boards use SerialUSB, some use Serial // #define SERIAL SerialUSB //Sparkfun Samd21 Boards #define SERIAL Serial //Adafruit, other Samd21 Boards // these are for easy handling of motor functions. const int Forward = LOW; const int Backward = HIGH; //************************************************************************** // global variables //************************************************************************** TaskHandle_t Handle_motorAzimuthTask; TaskHandle_t Handle_motorElevationTask; TaskHandle_t Handle_i2cCommsTask; TaskHandle_t Handle_monitorTask; byte azimuthMotorDir, azimuthMotorSpd, elevationMotorDir, elevationMotorSpd; //************************************************************************** // Can use these function for RTOS delays // Takes into account processor speed // Use these instead of delay(...) in rtos tasks //************************************************************************** void myDelayUs(int us) { vTaskDelay( us / portTICK_PERIOD_US ); } void myDelayMs(int ms) { vTaskDelay( (ms * 1000) / portTICK_PERIOD_US ); } void myDelayMsUntil(TickType_t *previousWakeTime, int ms) { vTaskDelayUntil( previousWakeTime, (ms * 1000) / portTICK_PERIOD_US ); } //************************************************************************** // General purpose functions to be used in severals tasks //************************************************************************** //***************************************************************** // Create a thread that outputs motor direction and speed demands for Azimuth angle //***************************************************************** static void runMotorAzimuth( void *pvParameters ) { SERIAL.println("Motor Azimuth: Started"); while(1) { azimuthMotorDir = Forward; azimuthMotorSpd = 255; SERIAL.println("Motor Azimuth >> Going forward"); myDelayMs(2000); azimuthMotorSpd = 0; SERIAL.println("Motor Azimuth >> Stopped"); myDelayMs(100); azimuthMotorDir = Backward; azimuthMotorSpd = 100; SERIAL.println("Motor Azimuth >> Going backward"); myDelayMs(5000); azimuthMotorSpd = 0; SERIAL.println("Motor Azimuth >> Stopped"); myDelayMs(100); } } //***************************************************************** // Create a thread that outputs motor direction and speed demands for Elevation angle //***************************************************************** static void runMotorElevation( void *pvParameters ) { int value; SERIAL.println("Motor Elevation: Started"); while(1) { elevationMotorDir = Backward; SERIAL.println("Motor Elevation >> Going backward"); for (value = 100 ; value <= 255; value += 5) { elevationMotorSpd = value; myDelayMs(200); } elevationMotorSpd = 0; elevationMotorDir = Forward; SERIAL.println("Motor Elevation >> Going forward"); for (value = 100 ; value <= 255; value += 5) { elevationMotorSpd = value; myDelayMs(200); } elevationMotorSpd = 0; } } //***************************************************************** // Create a thread that communicates to peripheral devices using I2C //***************************************************************** volatile double variable; static void i2cComms( void *pvParameters ) { SERIAL.println("Thread I2C Comms: Started"); while(1) { Wire.beginTransmission(2); // send to Uno which has motor driver mounted Wire.write(azimuthMotorDir); Wire.write(azimuthMotorSpd); Wire.write(elevationMotorDir); Wire.write(elevationMotorSpd); Wire.endTransmission(); myDelayMs(10); } } //***************************************************************** // Task will periodically print out useful information about the tasks running // Is a useful tool to help figure out stack sizes being used // Run time stats are generated from all task timing collected since startup // No easy way yet to clear the run time stats yet //***************************************************************** static char ptrTaskList[400]; //temporary string buffer for task stats void taskMonitor(void *pvParameters) { int measurement; SERIAL.println("Task Monitor: Started"); // run this task afew times before exiting forever while(1) { myDelayMs(10000); // print every 10 seconds SERIAL.flush(); SERIAL.println(""); SERIAL.println("****************************************************"); SERIAL.print("Free Heap: "); SERIAL.print(xPortGetFreeHeapSize()); SERIAL.println(" bytes"); SERIAL.print("Min Heap: "); SERIAL.print(xPortGetMinimumEverFreeHeapSize()); SERIAL.println(" bytes"); SERIAL.flush(); SERIAL.println("****************************************************"); SERIAL.println("Task ABS %Util"); SERIAL.println("****************************************************"); vTaskGetRunTimeStats(ptrTaskList); //save stats to char array SERIAL.println(ptrTaskList); //prints out already formatted stats SERIAL.flush(); SERIAL.println("****************************************************"); SERIAL.println("Task State Prio Stack Num Core" ); SERIAL.println("****************************************************"); vTaskList(ptrTaskList); //save stats to char array SERIAL.println(ptrTaskList); //prints out already formatted stats SERIAL.flush(); SERIAL.println("****************************************************"); SERIAL.println("[Stacks Free Bytes Remaining] "); measurement = uxTaskGetStackHighWaterMark( Handle_motorAzimuthTask ); SERIAL.print("Thread A: "); SERIAL.println(measurement); measurement = uxTaskGetStackHighWaterMark( Handle_motorElevationTask ); SERIAL.print("Thread B: "); SERIAL.println(measurement); measurement = uxTaskGetStackHighWaterMark( Handle_i2cCommsTask ); SERIAL.print("Thread C: "); SERIAL.println(measurement); measurement = uxTaskGetStackHighWaterMark( Handle_monitorTask ); SERIAL.print("Monitor Stack: "); SERIAL.println(measurement); SERIAL.println("****************************************************"); SERIAL.flush(); } // delete ourselves. // Have to call this or the system crashes when you reach the end bracket and then get scheduled. SERIAL.println("Task Monitor: Deleting"); vTaskDelete( NULL ); } //***************************************************************** void setup() { SERIAL.begin(115200); Wire.begin(); // this device is master delay(1000); // prevents usb driver crash on startup, do not omit this // while (!SERIAL) ; // Wait for serial terminal to open port before starting program SERIAL.println(""); SERIAL.println("******************************"); SERIAL.println(" Program start "); SERIAL.println("******************************"); SERIAL.flush(); // Set the led the rtos will blink when we have a fatal rtos error // RTOS also Needs to know if high/low is the state that turns on the led. // Error Blink Codes: // 3 blinks - Fatal Rtos Error, something bad happened. Think really hard about what you just changed. // 2 blinks - Malloc Failed, Happens when you couldn't create a rtos object. // Probably ran out of heap. // 1 blink - Stack overflow, Task needs more bytes defined for its stack! // Use the taskMonitor thread to help gauge how much more you need vSetErrorLed(ERROR_LED_PIN, ERROR_LED_LIGHTUP_STATE); // sets the serial port to print errors to when the rtos crashes // if this is not set, serial information is not printed by default vSetErrorSerial(&SERIAL); // Create the threads that will be managed by the rtos // Sets the stack size and priority of each task // Also initializes a handler pointer to each task, which are important to communicate with and retrieve info from tasks xTaskCreate(runMotorAzimuth, "Task Motor1", 256, NULL, tskIDLE_PRIORITY + 3, &Handle_motorAzimuthTask); xTaskCreate(runMotorElevation, "Task Motor2", 256, NULL, tskIDLE_PRIORITY + 3, &Handle_motorElevationTask); xTaskCreate(i2cComms, "Task I2C Comms", 256, NULL, tskIDLE_PRIORITY + 2, &Handle_i2cCommsTask); xTaskCreate(taskMonitor, "Task Monitor", 256, NULL, tskIDLE_PRIORITY + 1, &Handle_monitorTask); // Start the RTOS, this function will never return and will schedule the tasks. vTaskStartScheduler(); // error scheduler failed to start // should never get here while(1) { SERIAL.println("Scheduler Failed! \n"); SERIAL.flush(); delay(1000); } } //***************************************************************** // This is now the rtos idle loop // No rtos blocking functions allowed! //***************************************************************** void loop() { // Optional commands, can comment/uncomment below SERIAL.print("."); //print out dots in terminal, we only do this when the RTOS is in the idle state SERIAL.flush(); delay(100); //delay is interrupt friendly, unlike vNopDelayMS } //*****************************************************************
Useful tip! Comment out while (!SERIAL);
otherwise the code will get stuck in that line and wait for serial port to open for communication and not move further at all unless condition satisfied. This is not useful when you want to run the code standalone on MCU, i.e., not connected to a computer.
Following is the code to run on the Arduino UNO:
#include <Wire.h> #define MOTOR_AZIMUTH_SPD 10 #define MOTOR_AZIMUTH_DIR 12 #define MOTOR_ELEVATION_SPD 11 #define MOTOR_ELEVATION_DIR 13 int azimuthMotorDir, azimuthMotorSpd, elevationMotorDir, elevationMotorSpd; void setup() { Wire.begin(2); // this is slave device Wire.onReceive(receiveEvent); Serial.begin(115200); pinMode(MOTOR_AZIMUTH_SPD, OUTPUT); pinMode(MOTOR_AZIMUTH_DIR, OUTPUT); pinMode(MOTOR_ELEVATION_SPD, OUTPUT); pinMode(MOTOR_ELEVATION_DIR, OUTPUT); } void loop() { digitalWrite(MOTOR_AZIMUTH_DIR, azimuthMotorDir); analogWrite(MOTOR_AZIMUTH_SPD, azimuthMotorSpd); digitalWrite(MOTOR_ELEVATION_DIR, elevationMotorDir); analogWrite(MOTOR_ELEVATION_SPD, elevationMotorSpd); delay(1); } void receiveEvent(int howMany) { while(Wire.available()) // loop through all { azimuthMotorDir = Wire.read(); // receive direction Serial.print(azimuthMotorDir); Serial.print(" "); azimuthMotorSpd = Wire.read(); // receive speed Serial.print(azimuthMotorSpd); Serial.print(" "); elevationMotorDir = Wire.read(); Serial.print(elevationMotorDir); Serial.print(" "); elevationMotorSpd = Wire.read(); Serial.println(elevationMotorSpd); } }
And, here is the video in action!
So, now that we can make things move and sense and communicate all at (nearly) the same time, we can get started building the final solution. Let's build the physical device first!