In my previous blog, I had shown the physical sytem containing a mirror mounted on two axis gimbal. Now, to know where we are pointing, we need sensors on each axis (azimuth and elevation).
Sensor integration:
For elevation, I used the Grove 3-axis accelerometer. However, for azimuth (i.e. yaw) I couldn't use the same accelerometer since it cannot give that infomation when we rotate around axis that points down towards Earth or up towards sky. I could perhaps use GPS but that might be an expensive solution. So, I used potentiometer on azimuth axis. Here, are some pictures showing those sensors.
I did not have belt that matched the gear on the azimuth shaft so I just used a gear that fitted on the potentiometer shaft and then put a rubber band around. The first attempt had failed where I had wider rubber band initially. The problem with rubber band is that it first stretches on the side that turns and doesn't transfer the torque enough to turn the potentiometer, causing backlash. So I thought I would sew thread into it to help transfer the load easily without stretching too much. But, those holes that were created made it snap! 
Picture showing rubber band that failed.
In the end, I used much smaller (radius wise) rubber band and put it around and also I put a thick rubber (removed from printer roller) because it kept slipping on the plastic shaft (refer to pitcture showing blue rubber band).
To know whether we have focus of the mirror where needed, I mounted 3 photo resistors on a garden wire and pointed them towards the mirror. Those sensors were wired with 220 ohm resistors each in series with each photo resistor so that voltage drop across the sensor could be measured that correspond to brightness of the light. This picture shows the set up of those sensors.
And here is the data showing trying out those photo sensors. I took the torch on mobile phone and went around each sensors to see their responses. Later, I moved the mobile phone close to where the mirror is and then waved around in a fashion how I expect the miror would move. In fact, it showed the response I was expecting, clear enough signals to distinguish the focus between left, right and top sensors.
Sensor Calibration:
For photo sensors, I am simply using the ADC counts directly as I do not have means to calibrate ADC counts to light in lumens. Also, for my purposes, I do not need to convert those into lumens because I am only using these values for comparing to each other when mirror focusing is required.
For elevation angle, I am using Y and Z axis acceleration (in g). They are calibrated values coming out of the accelerometer so it is easier to calculate angles. The formula used to convert acceleration values into elevation angle is,
elevationAngle = (180/pi) * tan-1(-y/z); // in degrees
The potentiometer I have used for azimuth angle is 10kOhm and therefore it has nonlinear relationship to the angle. Here is the graph showing ADC count from analogRead versus angle of the potentiometer dial. The range from 0 - 800, i.e. 277° - 90° seems nearly linear and therefore I used that range only and besides, that is sufficiently large for the coverage in azimuth. If more range is required, larger gear on the potentiometer shaft should increase the range.
The equation to convert potentiometer reading in this project is,
azimuthAngle = (-0.2273 * ADC Count) + 277; // in degrees.
And here is the test results showing all sensos in action. I took a video showing that process.
Here is the graph that was generated from the data shown in the video.
Controller Code:
Combining all components in the system, let me show you how it is all arranged in a system architecture diagram.
Here, power supply is not shown and there may be a few buttons and LEDs that will be included in the future but are not shown.
The control scheme is shown as below.
So, basically, the setpoint is the value of photo sensors to maintain to a particular level. When Earth moves, the sun position in the sky would shift, causing difference in photo sensor values. This is fedback to the controller which, based on the direction (defined by the photo sensor array), would demand movement of mirror in the corresponding direction. This new demand is angle (azimuth and elevation) demands. These demands are fed to PID regulator which will generate output demands in terms of angular speed of the motors. This demand is what goes to the motor driver to generate movement. D-term in PID is set to 0 since it is not needed and might cause unnecessary oscillations when signals are noisy (elevation angle is particularly noisy). Proportional gain is responsible for the responsiveness of the system, as in, higher value of proportional gain will cause system to move faster to respond to the error in angle (demanded versus actual) but will not be able to achieve the demanded angle (there will be steady-state error). We will use Integral term from the PID to get rid of this steady state error. Tuning of these gains is necessary when commissioning the system to result in acceptable behaviour from the system. However, we will start with reasonably small values to avoid disappointments from breaking the system.
Now, here is the code to implement all that plus an additional bit for state machine that I have not explained above.
CAUTION: the following code has not yet been tested fully!
//**************************************************************************
// FreeRtos on Samd21
// Template by Scott Briscoe
//
// Code written by Hitesh Boghani
// Program for the Sunray bender
// October 2023
//**************************************************************************
#include <FreeRTOS_SAMD21.h>
#include <Wire.h>
#include "DHT.h"
#include "MMA7660.h"
#include <PIDController.h>
#include <StopWatch.h>
//**************************************************************************
// Type Defines and Constants
//**************************************************************************
#define ERROR_LED_PIN 13 //Led Pin: Typical Arduino Board
#define TOP_PHOTOSENSOR 0 // top photo resistor
#define LEFT_PHOTOSENSOR 1 // bottom left photo resistor
#define RIGHT_PHOTOSENSOR 2 // bottom right photo resistor
#define ELEVATION_POT 3 // elevation angle
#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_stateMachineTask;
TaskHandle_t Handle_controlLawTask;
TaskHandle_t Handle_ioTask;
TaskHandle_t Handle_serialCommsTask;
TaskHandle_t Handle_i2cCommsTask;
TaskHandle_t Handle_monitorTask;
byte azimuthMotorDir, azimuthMotorSpd, elevationMotorDir, elevationMotorSpd, state, timerState;
float DHTVals[2] = {0};
int azimuthAngle, elevationAngle, azimuthAngleDemand, elevationAngleDemand, elevationAngleSleep;
int topPhotoSensor, leftPhotoSensor, rightPhotoSensor;
int photoMinTrack = 1023; // this is the minimum photo value we need to track
double azimuthKp = 1, azimuthKi = 0.1, elevationKp = 1, elevationKi = 0.1;
uint16_t delaySMTask = 10, delayCLTask = 10, delayIOTask = 10, delaySerialTask = 10, delayI2CTask = 10;
uint64_t timeS = 0;
int searchComplete = 0, searchTimeGap = 900;
StopWatch swAzimuthMotor(StopWatch::SECONDS);
StopWatch swSearchState(StopWatch::SECONDS);
enum SunrayBenderStates {
IDLE = 0,
SEARCH = 10,
TRACK = 20,
SLEEP = 30,
ERROR = 99
};
SunrayBenderStates sunrayBenderState = IDLE; // initialise state variable with IDLE
DHT dht(DHT20);
MMA7660 accelemeter;
PIDController pidAzimuth;
PIDController pidElevation;
//**************************************************************************
// 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
//**************************************************************************
int minimum(int a, int b, int c) {
return a < b ? (a < c ? a : c) : (b < c ? b : c);
}
int maximum(int a, int b, int c) {
return a > b ? (a > c ? a : c) : (b > c ? b : c);
}
int average(int a, int b, int c) {
return (int) (((float)a + (float) b + (float) c) / 3);
}
//*****************************************************************
// Create a thread that runs state machine
// States:
// - SLEEP
// - IDLE
// - SEARCH
// - TRACK
// - ERROR
//*****************************************************************
static void stateMachine( void *pvParameters )
{
// SERIAL.println("State Machine: Started");
int photoThreshold = 1015; // above this value, sunray bender will go to sleep
int photoMinDelta = 200; // above this value, overall lighting is deemed to have changed, eg. cloud coming over
int photoDriftTrig = 300; // above this value, the sun has drifted
int photoAverage;
int photoMin;
int photoMax;
int photoDelta;
while(1)
{
photoAverage = average(topPhotoSensor, leftPhotoSensor, rightPhotoSensor);
photoMin = minimum(topPhotoSensor, leftPhotoSensor, rightPhotoSensor);
photoMax = maximum(topPhotoSensor, leftPhotoSensor, rightPhotoSensor);
photoDelta = abs(photoMinTrack - photoAverage);
switch (sunrayBenderState)
{
case IDLE:
/* leave the mirror where it is unless lighting condition change, i.e. Earth has moved */
{
// do enter search task if the photoMinDelta has moved to (probably) another value
// do not enter search task if we did search recently
// do not enter search task at night or when raining, i.e. dark condition
if (photoDelta > photoMinDelta && (int)swSearchState.elapsed() >= searchTimeGap && photoAverage < photoThreshold) {
swSearchState.stop(); swSearchState.reset();
sunrayBenderState = SEARCH;
photoMinTrack = 1023; // assume dark condition
} else if ((photoMax - photoMin) > photoDriftTrig){
sunrayBenderState = TRACK;
}
if (photoAverage > photoThreshold) {
sunrayBenderState = SLEEP;
}
break;
}
case SEARCH:
/* search for minimum for certain time */
{
if (swSearchState.isStopped()) {
/* make sure to reset the clock if someone hasn't bothered to do that */
swSearchState.reset();
swSearchState.start();
}
int photoNewMin = photoMin;
if (photoNewMin < photoMinTrack) {
// grab new value of photo value to maintain
photoMinTrack = photoNewMin;
// grab elevation and azimuth angles
elevationAngleDemand = elevationAngle;
azimuthAngleDemand = azimuthAngle;
}
if (searchComplete) {
sunrayBenderState = TRACK;
searchComplete = 0;
}
break;
}
case TRACK:
/* keep following the minimum unless there is new minimum for which trigger search */
{
if ((photoMax - photoMin) < photoDriftTrig){
sunrayBenderState = IDLE;
}
break;
}
case SLEEP:
/* turn the mirror down - this is to make sure not to dazzle neighbours if the mirror position is arbitrary and sun comes out */
{
if(abs(elevationAngleDemand - elevationAngle) < 10){
sunrayBenderState = IDLE;
}
break;
}
case ERROR:
/* motor stalled meaning stuck and not moving in either direction, sensors unplugged */
// TODO: wait for reset signal from operator
{
// if reset then sunrayBenderState = IDLE;
break;
}
default:
{
state = IDLE;
break;
}
}
myDelayMs(delaySMTask);
}
}
//*****************************************************************
// Create a thread that runs control law
//*****************************************************************
static void controlLaw( void *pvParameters )
{
// SERIAL.println("Control Law: Started");
while(1) {
// Work out demands based on Suray Bender state
switch (sunrayBenderState)
{
case SEARCH:
/* do the search */
{
for (int i = 0; i < 11; i++) {
azimuthAngleDemand = 120 + i*10;
// azimuthAngleDemand = 120; azimuthAngleDemand < 230; azimuthAngleDemand+10
do {
myDelayMs(100); // wait until demand is not met
} while (abs(azimuthAngleDemand - azimuthAngle) < 2);
for (int j = 0; j < 6; j++) {
elevationAngleDemand = 120 + i*20;
// elevationAngleDemand = 120; elevationAngleDemand < 240; elevationAngleDemand+20
do {
myDelayMs(100); // wait until demand is not met
} while (abs(elevationAngleDemand - elevationAngle) < 5);
}
for (int j = 6; j > 0; j--) {
elevationAngleDemand = 240 - i*20;
// elevationAngleDemand = 240; elevationAngleDemand > 120; elevationAngleDemand - 20
do {
myDelayMs(100); // wait until demand is not met
} while (abs(elevationAngleDemand - elevationAngle) < 5);
}
}
}
searchComplete = 1;
break;
case TRACK:
/* follow the sun */
{
// look at delta between top and maximum of left and right for elevation demand
if ((topPhotoSensor - leftPhotoSensor - rightPhotoSensor) > 10) {
elevationAngleDemand = elevationAngle + 5;
}
if ((topPhotoSensor - leftPhotoSensor - rightPhotoSensor) < 10) {
elevationAngleDemand = elevationAngle - 5;
}
// look at delta between left and right photo sensors for azimuth demand
if((leftPhotoSensor - rightPhotoSensor) > 10 && azimuthAngle < 225) { // check if in range
azimuthAngleDemand = azimuthAngle + 5;
}
if((leftPhotoSensor - rightPhotoSensor) < 10 && azimuthAngle > 125) { // check if in range
azimuthAngleDemand = azimuthAngle - 5;
}
break;
}
case SLEEP:
/* put down the mirror */
{
// follow elevation angle demand
elevationAngleDemand = elevationAngleSleep;
break;
}
default:
/* do nothing here */
{
break;
}
}
// work out speed demand based on PID regulator
if (sunrayBenderState != IDLE || sunrayBenderState != ERROR) {
pidAzimuth.setpoint(azimuthAngleDemand);
azimuthMotorSpd = pidAzimuth.compute(azimuthAngle);
pidElevation.setpoint(elevationAngleDemand);
elevationMotorSpd = pidElevation.compute(elevationAngle);
azimuthMotorDir = azimuthMotorSpd < 0? Backward : Forward;
elevationMotorDir = elevationMotorSpd < 0? Backward : Forward;
} else {
azimuthMotorSpd = 0;
elevationMotorSpd = 0;
}
myDelayMs(delayCLTask);
}
}
//*****************************************************************
// Create a thread that communicates to peripheral devices using I2C
//*****************************************************************
static void i2cComms( void *pvParameters )
{
// SERIAL.println("Thread I2C Comms: Started");
int8_t x;
int8_t y;
int8_t z;
while(1)
{
dht.readTempAndHumidity(DHTVals);
// dtostrf(DHTVals[0], 5, 2, buff);
// SERIAL.print("Humidity: ");
// SERIAL.print(buff);
// SERIAL.print("% | ");
// strcpy(buff, "\0");
// dtostrf(DHTVals[1], 5, 2, buff);
// SERIAL.print("Temperature: ");
// SERIAL.print(buff);
// SERIAL.print("°C | ");
accelemeter.getXYZ(&x,&y,&z);
float elevation;
elevation = atan2(-y, z) * 180/M_PI + 180; // offset applied
elevationAngle = ((int) elevation + 360) % 360; // turn ±180° into ±360°
Wire.beginTransmission(2); // send to Uno which has motor driver mounted
Wire.write(azimuthMotorDir);
Wire.write(abs(azimuthMotorSpd));
Wire.write(elevationMotorDir);
Wire.write(abs(elevationMotorSpd));
Wire.endTransmission();
myDelayMs(delayI2CTask);
}
}
//*****************************************************************
// Create a thread that does measurements
//*****************************************************************
static void io( void *pvParameters )
{
// SERIAL.println("Thread Sensors: Started");
while(1)
{
// char buff[16];
// strcpy(buff, "\0");
topPhotoSensor = analogRead(TOP_PHOTOSENSOR);
// dtostrf(topPhotoSensor, 5, 2, buff);
// SERIAL.print("topPhotoSensor: ");
// SERIAL.print(topPhotoSensor);
// SERIAL.print(",");
// strcpy(buff, "\0");
leftPhotoSensor = analogRead(LEFT_PHOTOSENSOR);
// dtostrf(leftPhotoSensor, 5, 2, buff);
// SERIAL.print("leftPhotoSensor: ");
// SERIAL.print(leftPhotoSensor);
// SERIAL.print(",");
// strcpy(buff, "\0");
rightPhotoSensor = analogRead(RIGHT_PHOTOSENSOR);
// dtostrf(rightPhotoSensor, 5, 2, buff);
// SERIAL.print("rightPhotoSensor: ");
// SERIAL.print(rightPhotoSensor);
// SERIAL.println("");
azimuthAngle = (int) (((float) analogRead(ELEVATION_POT)) * -0.2273 + 277);
myDelayMs(delayIOTask);
}
}
//*****************************************************************
// Create a thread that handles serial communication
// Modes:
// - debug, to print all messages
// - log, to print data in parseable format disable printing everything else
//*****************************************************************
static void serialComms( void *pvParameters )
{
// SERIAL.println("Thread Serial Comms: Started");
byte serialInput, logStatus = 0, debugStatus = 0;
while(1)
{
serialInput = SERIAL.read();
if (serialInput == 1) {
logStatus = 1;
debugStatus = 0;
}
if (serialInput == 10) {
logStatus = 0;
}
if (serialInput == 9) {
debugStatus = 1;
logStatus = 0;
}
if (serialInput == 90) {
debugStatus = 0;
}
if (logStatus) {
SERIAL.print(azimuthAngle);
SERIAL.print(",");
SERIAL.print(elevationAngle);
SERIAL.print(",");
SERIAL.print(topPhotoSensor);
SERIAL.print(",");
SERIAL.print(leftPhotoSensor);
SERIAL.print(",");
SERIAL.print(rightPhotoSensor);
SERIAL.print(",");
SERIAL.print(photoMinTrack);
SERIAL.print(",");
SERIAL.print(azimuthAngleDemand);
SERIAL.print(",");
SERIAL.print(elevationAngleDemand);
SERIAL.print(",");
SERIAL.print(azimuthMotorSpd);
SERIAL.print(",");
SERIAL.print(elevationMotorSpd);
SERIAL.println("");
}
if (debugStatus) {
// print all diverted messages to serial from here.
// function SerialPrintDebug();
}
myDelayMs(1);
}
}
//*****************************************************************
// 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_controlLawTask );
SERIAL.print("motorAzimuthTask : ");
SERIAL.println(measurement);
measurement = uxTaskGetStackHighWaterMark( Handle_serialCommsTask );
SERIAL.print("motorElevationTask: ");
SERIAL.println(measurement);
measurement = uxTaskGetStackHighWaterMark( Handle_i2cCommsTask );
SERIAL.print("i2cCommsTask: ");
SERIAL.println(measurement);
measurement = uxTaskGetStackHighWaterMark( Handle_monitorTask );
SERIAL.print("monitorTask: ");
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
pidAzimuth.begin();
pidElevation.begin();
pidAzimuth.tune(azimuthKp, azimuthKi, 0);
pidElevation.tune(elevationKp, elevationKi, 0);
pidAzimuth.limit(0, 100);
pidElevation.limit(0, 100);
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(controlLaw, "Task Control Law", 512, NULL, tskIDLE_PRIORITY + 3, &Handle_controlLawTask);
xTaskCreate(stateMachine, "Task State Machine", 512, NULL, tskIDLE_PRIORITY + 4, &Handle_stateMachineTask);
xTaskCreate(io, "Task IO", 512, NULL, tskIDLE_PRIORITY + 3, &Handle_ioTask);
xTaskCreate(i2cComms, "Task I2C Comms", 512, NULL, tskIDLE_PRIORITY + 2, &Handle_i2cCommsTask);
xTaskCreate(serialComms, "Task Serial Comms", 512, NULL, tskIDLE_PRIORITY + 1, &Handle_serialCommsTask);
// 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
}
//*****************************************************************
Apart from the Arduino, Wire and FreeRTOS_SAMD21 libraries, I am also using DHT, MMA7660, PIDController and StopWatch libraries to make things easier.
I will skip the usual global variables but task handlers perhaps need mention. I have created 5 tasks (6th task, monitorTask is actually from the template) that will be scheduled by the RTOS to run in parallel (nearly parallel).
In the task for state machine, the code will enter into IDLE state by default after entering into the forever while loop. Here is the visual for the state machine that I have implemented.
Currently, I have not implemented the ERROR state and therefore it is shown in grey in above picture.
In the control law task, I am working out angle demands based on the state of the Sunray Bender and then proceed to calculate the output that needs to be applied based on the PID regulator.
Sensor values from all the sensors are gathered by IO task and I2C task.
Finally, serialComms task handles serial communication. In particular, I am interested in logging the data in a parsable format so I have created two different modes, one to log the data and other for debugging to see what the code is doing but not both at the same time. The debug part is not yet implemented fully.
Another part that is not implemented is to know when it might be raining. I intend to use the temperature and humidity from DHT and combine with photo sensor value to discern the rainy condition and put the tracker in IDLE state to do nothing. I will implement this additional part later after the main functions work properly.
The motor driver is mounted on Arduino Uno so this is the code that runs on Uno:
// code to run on Uno for DRI0017 motor driver
#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);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
}
void loop() {
digitalWrite(MOTOR_AZIMUTH_DIR, azimuthMotorDir);
analogWrite(MOTOR_AZIMUTH_SPD, setMotorSpeed(azimuthMotorSpd));
digitalWrite(MOTOR_ELEVATION_DIR, elevationMotorDir);
analogWrite(MOTOR_ELEVATION_SPD, setMotorSpeed(elevationMotorSpd));
digitalWrite(7, HIGH);
digitalWrite(8, LOW);
delay(1);
}
int setMotorSpeed(int speed ) {
/*turn 0-100 range into 0-255 range */
int pwmValue = (int)(255.0 * ((float)speed / 100.0));
// coerce the value into 0-255 range
if (pwmValue > 255) {
pwmValue = 255;
}
if (pwmValue < 0) {
pwmValue = 0;
}
return pwmValue; //PWM
}
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);
}
}
In this code, I have put protection for the speed input and also scaled it from 0-100 to 0-255.
In the next blog, I will be testing this code and hopefully get the system in a fully autonomously working state! 
Previous Next
