I have abandoned that project.
Here is the code that the project turned into:
//////// http://www.iotsharing.com/2017/09/how-to-use-arduino-esp32-can-interface.html
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/event_groups.h"
#include <HardwareSerial.h>
#include "esp32-hal-ledc.h"
#include "TFMini.h"
// #include "ESP32_MPU9250.h"
////////////////////////////////
/////////* create event group */
EventGroupHandle_t eg;
/////* define event bits */
//#define evtSendCAN_Buss ( 1 << 0 ) // 1
#define evtGetIMU ( 1 << 1 ) // 10
#define evtTweakServoX ( 1 << 2 ) // 100
//#define evtCheckAlarmCondition ( 1 << 3 ) // 1000
#define evtLIDAR_ServoAspectChange ( 1 << 4 ) // 10000
// #define evtTweakServoLIDAR ( 1 << 5 ) // 100000
//#define AnalogVoltReadTask ( 1 << 6 ) // 1000000
#define evtSendSerialToBrain ( 1 << 7 ) // 10000000
#define evtDoLIDAR ( 1 << 8 ) // 100000000
#define evtTweakServoY ( 1 << 9 ) // 1000000000
#define evtLIDAR_Is_OK ( 1 << 10 ) // 10000000000
#define evtSendLIDAR_Info_For_ALARM ( 1 << 11 ) // 100000000000
////////#define OneMilliGroupBits ( evtSendOut_SEMAPHORE )
////////////////////////////////////////////////
////////////////////////////////////////////////
HardwareSerial SerialTFMini( 1 );
HardwareSerial SerialBrain( 2 );
////// serial(1) = pin27=RX green, pin26=TX white
////// serial(2) = pin16=RXgreen , pin17=TX white
// myMPU9250 mpu9250( 3 );
//////
TFMini tfmini;
//////
#define TimerDivider 80
#define TaskCore1 1
#define TaskCore0 0
#define TIMER_FOUR 3
#define OneK 1000
#define TaskStack10K5 10500
#define TaskStack15K 15000
#define TaskStack20K 20000
#define TaskStack30K 30000
#define TaskStack40K 40000
#define Priority2 2
#define Priority3 3
#define Priority4 4
#define SerialDataBits 115200
#define StringBufferSize 50
#define Servo_LIDAR_Pin 4 // white wire
#define iLIDAR_Posit90 1475 //using writeMicroseconds
#define XaxisServoPin 33 // blue wire
#define iX_Posit90 1500 //using writeMicroseconds
#define YaxisServoPin 32 // yellow wire
#define iY_Posit90 1435 //using writeMicroseconds
#define MINservo 1349 // 45 degrees
#define MAXservo 1747 // 135 degrees
#define ScanPoints 62
#define BlinkLED 2
#define LIDAR_Max_Distance 600
#define COUNT_LOW 1638
#define COUNT_HIGH 7864
#define TIMER_WIDTH 16
#define REFRESH_USEC 20000
#define Channel_Y 0
#define Channel_LIDAR 2
#define Channel_X 4
#define TIMER_FREQUENCY 50
#define MPU_int_Pin 34
//////
//////*************************************************
SemaphoreHandle_t sema_LIDAR_FOR_ALARM;
SemaphoreHandle_t sema_SendSerialToBrain;
SemaphoreHandle_t sema_LIDAR;
SemaphoreHandle_t sema_DoTheServoTorque;
/////////////////////////////////////
TickType_t ServoDelay60mS = pdMS_TO_TICKS( 60 );
TickType_t ServoDelay40mS = pdMS_TO_TICKS( 40 );
TickType_t ServoDelay12mS = pdMS_TO_TICKS( 12 );
TickType_t xZeroTicksToWait = 0;
TickType_t xSemaphoreTicksToWait = 15;
TickType_t QueueReceiveDelayTime = 10;
/////////////////////////////////////
////////*********************************************
////// Recommended servo pins include 2,4,12-19,21-23,25-27,32-33
/*
** ledc: 0 => Group: 0, Channel: 0, Timer: 0
** ledc: 1 => Group: 0, Channel: 1, Timer: 0
** ledc: 2 => Group: 0, Channel: 2, Timer: 1
** ledc: 3 => Group: 0, Channel: 3, Timer: 1
** ledc: 4 => Group: 0, Channel: 4, Timer: 2
** ledc: 5 => Group: 0, Channel: 5, Timer: 2
** ledc: 6 => Group: 0, Channel: 6, Timer: 3
** ledc: 7 => Group: 0, Channel: 7, Timer: 3
** ledc: 8 => Group: 1, Channel: 8, Timer: 0
** ledc: 9 => Group: 1, Channel: 9, Timer: 0
** ledc: 10 => Group: 1, Channel: 10, Timer: 1
** ledc: 11 => Group: 1, Channel: 11, Timer: 1
** ledc: 12 => Group: 1, Channel: 12, Timer: 2
** ledc: 13 => Group: 1, Channel: 13, Timer: 2
** ledc: 14 => Group: 1, Channel: 14, Timer: 3
** ledc: 15 => Group: 1, Channel: 15, Timer: 3
*/
SemaphoreHandle_t sema_LIDAR_INFO;
QueueHandle_t xQ_LIDAR_INFO;
struct stu_LIDAR_INFO
{
int Range[66];
int CurrentCell;
bool ServoSweepUp = true;
} x_LIDAR_INFO;
////////*********************************************
////////*************************************************
QueueHandle_t xQ_X_INFO;
QueueHandle_t xQ_Y_INFO;
SemaphoreHandle_t sema_X;
SemaphoreHandle_t sema_Y;
// SemaphoreHandle_t sema_Z;
float roll = 0.0;
float pitch = 0.0;
float yaw = 0.0;
////////*************************************************
QueueHandle_t xQ_LIDAR_FOR_ALARM;
struct stu_LIDAR_MinMax_INFO
{
int MIN = 1000;
int MAX = 2000;
int LOW_Range = 1413;
int HIGH_Range = 1537;
int MEDIUM_Range = 1475;
int WIDTH = 62;
} x_LIDAR_MinMax;
////////*************************************************
////////////////////////////////////////////////
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
float deltat = 0.0f; // integration interval for both filter schemes
#define Kp 7.50f
// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 1.7f
////////*************************************************
////////*************************************************
bool LIDAR_Is_OK = false;
////*************************************************
float timer_width_ticks = pow( 2, TIMER_WIDTH );
////*************************************************
TaskHandle_t xHANDLE_LIDAR_ServoAspectChange = NULL;
TaskHandle_t xHANDLE_DoLIDAR = NULL;
TaskHandle_t xHANDLE_SendLIDAR_InfoSerialToBrain = NULL;
TaskHandle_t xHANDLE_SendSerialToBrain = NULL;
TaskHandle_t xHANDLE_TweakServoX = NULL;
TaskHandle_t xHANDLE_TweakServoY = NULL;
TaskHandle_t xHANDLE_GetIMU = NULL;
TaskHandle_t xHANDLE_TweakServoLIDAR = NULL;
////*************************************************
////*************************************************
void triggerGet_IMU()
{
BaseType_t xHigherPriorityTaskWoken;
xEventGroupSetBitsFromISR(eg, evtGetIMU, &xHigherPriorityTaskWoken);
}
//////////////////////////////////////////////////////////////////////
void setup()
{
////
// create event group, make before timer trigger
eg = xEventGroupCreate();
Serial.begin( SerialDataBits );
SerialBrain.begin( SerialDataBits );
SerialTFMini.begin( SerialDataBits, SERIAL_8N1, 27, 26 );
// Initialize the TFMini LIDAR
tfmini.begin(&SerialTFMini);
vTaskDelay( pdMS_TO_TICKS( 3 ) );
// Initialize single measurement mode with external trigger
tfmini.setSingleScanMode();
////
//// pinMode( BlinkLED, OUTPUT );//blink LED at pin 2
////
ledcSetup ( Channel_X, TIMER_FREQUENCY, TIMER_WIDTH ); //
ledcAttachPin ( XaxisServoPin, Channel_X ); //
ledcWrite (Channel_X, usToTicks ( iX_Posit90 ) ); //
vTaskDelay( ServoDelay60mS );
////
ledcSetup( Channel_LIDAR, TIMER_FREQUENCY, TIMER_WIDTH); //
ledcAttachPin( Servo_LIDAR_Pin, Channel_LIDAR); //
ledcWrite( Channel_LIDAR, usToTicks ( iLIDAR_Posit90 ) );
vTaskDelay( ServoDelay60mS );
////
ledcSetup ( Channel_Y, TIMER_FREQUENCY, TIMER_WIDTH ); //
ledcAttachPin ( YaxisServoPin, Channel_Y ); //
ledcWrite (Channel_Y, usToTicks ( iY_Posit90 ) ); //
vTaskDelay( ServoDelay60mS );
// make semaphores used in timer function before timer function begins
sema_X = xSemaphoreCreateMutex();
sema_Y = xSemaphoreCreateMutex();
sema_LIDAR_FOR_ALARM = xSemaphoreCreateMutex();
sema_SendSerialToBrain = xSemaphoreCreateMutex();
///////////// create queues
// queue length, queue item size
xQ_LIDAR_INFO = xQueueCreate( 1, sizeof(struct stu_LIDAR_INFO) ); // sends a queue copy
xQ_X_INFO = xQueueCreate( 1, sizeof( float ) ); // sends a queue copy
xQ_Y_INFO = xQueueCreate( 1, sizeof( float ) ); // sends a queue copy
xQ_LIDAR_FOR_ALARM = xQueueCreate( 1, sizeof(struct stu_LIDAR_INFO) ); // sends a queue copy
//////////////////////// freeRTOS TASKS
//////////////////////////////// TASK CORE 1 ///////////////////////////////////////////////////////////////////////////////////////////////////
xTaskCreatePinnedToCore( fLIDAR_ServoAspectChange, "fLIDAR_ServoAspectChange", TaskStack20K, NULL, Priority4, &xHANDLE_LIDAR_ServoAspectChange, TaskCore1 ); // assigned to core
sema_LIDAR_INFO = xSemaphoreCreateMutex();
xSemaphoreGive( sema_LIDAR_INFO );
xTaskCreatePinnedToCore( fDoLIDAR, "fDoLIDAR", TaskStack20K, NULL, Priority4, &xHANDLE_DoLIDAR, TaskCore1 ); //assigned to core
sema_LIDAR = xSemaphoreCreateMutex();
xSemaphoreGive ( sema_LIDAR );
xSemaphoreGive ( sema_LIDAR_FOR_ALARM );
xTaskCreatePinnedToCore( fSendLIDAR_InfoSerialToBrain, "fSendSerialToBrain", TaskStack20K, NULL, Priority3, &xHANDLE_SendLIDAR_InfoSerialToBrain, TaskCore1 ); // assigned to core
// add in task handles and check if null
xTaskCreatePinnedToCore( fLIDAR_Is_OK, "fLIDAR_Is_OK", TaskStack20K, NULL, Priority3, NULL, TaskCore1 ); // assigned to core
//////////////////////////////// TASK CORE 0 ///////////////////////////////////////////////////////////////////////////////////////////////////
xTaskCreatePinnedToCore( fSendSerialToBrain, "fSendSerialToBrain", TaskStack20K, NULL, Priority3, &xHANDLE_SendSerialToBrain, TaskCore0 ); // assigned to core
xTaskCreatePinnedToCore( fTweakServoX, "fTweakServoXY", TaskStack20K, NULL, Priority3, &xHANDLE_TweakServoX, TaskCore1 ); // assigned to core
xTaskCreatePinnedToCore( fTweakServoY, "fTweakServoXY", TaskStack20K, NULL, Priority3, &xHANDLE_TweakServoY, TaskCore1 ); // assigned to core
xTaskCreatePinnedToCore ( fGetIMU, "v_getIMU", TaskStack40K, NULL, Priority4, &xHANDLE_GetIMU, TaskCore0 );
// vTaskDelay ( 300 );
// xEventGroupSetBits( eg, evtGetIMU );
} // setup()
////******************************************
void loop() {} // loop
////******************************************
////******************************************
int usToTicks(int usec)
{
return (int)((float)usec / ((float)REFRESH_USEC / (float)timer_width_ticks) * (((float)TIMER_FREQUENCY) / 50.0));
} // int usToTicks(int usec)
////****************************************
void fLIDAR_Is_OK ( void *pvParameters )
{
String sSerial = "<$>";
TickType_t xLastWakeTime;
const TickType_t xFrequency = pdMS_TO_TICKS( 1000 );
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
for ( ;; )
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
bool LIDAR_OK = true;
if ( xSemaphoreTake( sema_SendSerialToBrain, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, no wait
{
if (xHANDLE_LIDAR_ServoAspectChange == NULL )
{
Serial.println ( " xHANDLE_LIDAR_ServoAspectChange " );
LIDAR_OK = false;
}
if ( xHANDLE_DoLIDAR == NULL )
{
Serial.println ( " xHANDLE_DoLIDAR " );
LIDAR_OK = false;
}
if ( xHANDLE_SendLIDAR_InfoSerialToBrain == NULL )
{
Serial.println ( " xHANDLE_SendLIDAR_InfoSerialToBrain " );
LIDAR_OK = false;
}
if ( xHANDLE_SendSerialToBrain == NULL )
{
Serial.println ( " xHANDLE_SendSerialToBrain " );
LIDAR_OK = false;
}
if ( xHANDLE_TweakServoX == NULL )
{
Serial.println ( " xHANDLE_TweakServoX " );
LIDAR_OK = false;
}
if ( xHANDLE_TweakServoY == NULL )
{
Serial.println ( " xHANDLE_TweakServoY " );
LIDAR_OK = false;
}
if ( xHANDLE_GetIMU == NULL )
{
Serial.println ( " xHANDLE_GetIMU " );
LIDAR_OK = false;
}
if ( LIDAR_OK )
{
SerialBrain.println ( sSerial );
}
}
xSemaphoreGive ( sema_SendSerialToBrain );
xLastWakeTime = xTaskGetTickCount();
}
vTaskDelete( NULL );
}
//******************************************
void fSendLIDAR_InfoSerialToBrain( void *pvParameters )
{
struct stu_LIDAR_INFO pxLIDAR_INFO;
for ( ;; )
{
xEventGroupWaitBits (eg, evtGetIMU, pdTRUE, pdTRUE, portMAX_DELAY);
xSemaphoreTake( sema_LIDAR_FOR_ALARM, xSemaphoreTicksToWait );
xQueueReceive ( xQ_LIDAR_FOR_ALARM, &pxLIDAR_INFO, QueueReceiveDelayTime );
xSemaphoreGive( sema_LIDAR_FOR_ALARM );
int CellCount = 1 ;
// Serial.print ( " fSendLIDAR_InfoSerialToBrain " );
// // send LIDAR info for alarm
String sSerial = "";
sSerial.reserve ( 300 );
sSerial.concat( "<#," ); //sentence begin
sSerial.concat( String(ScanPoints) + "," );
sSerial.concat( String(pxLIDAR_INFO.ServoSweepUp) + "," ); // get direction of scan
for ( CellCount; CellCount <= ScanPoints; CellCount++ )
{
sSerial.concat( String(pxLIDAR_INFO.Range[CellCount]) + "," );
}
sSerial.concat( ">" ); //sentence end
vTaskDelay( 10 );
SerialBrain.println ( sSerial );
}
vTaskDelete( NULL );
} // void fSendLIDAAR_InfoSerialToBrain( void *pvParameters )
////
void fSendSerialToBrain( void *pvParameters )
{
struct stu_LIDAR_INFO pxLIDAR_INFO;
xSemaphoreGive ( sema_SendSerialToBrain );
String sSerial;
sSerial.reserve ( 300 );
for (;;)
{
xEventGroupWaitBits (eg, evtSendSerialToBrain, pdTRUE, pdTRUE, portMAX_DELAY);
// Serial.println ( " fSendSerialToBrain " );
if ( xSemaphoreTake( sema_SendSerialToBrain, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, no wait
{
int CellCount = 1;
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
xQueueReceive ( xQ_LIDAR_INFO, &pxLIDAR_INFO, QueueReceiveDelayTime );
xSemaphoreGive( sema_LIDAR_INFO );
sSerial.concat( "<!," ); //sentence begin
sSerial.concat( String(ScanPoints) + "," );
for ( CellCount; CellCount <= ScanPoints; CellCount++ )
{
sSerial.concat( String(pxLIDAR_INFO.Range[CellCount]) + "," );
}
sSerial.concat( ">" ); //sentence end
// Serial.println ( sSerial );
SerialBrain.println ( sSerial );
// ////
// xEventGroupSetBits( eg, evtSendLIDAR_Info_For_ALARM );
// xSemaphoreGive ( sema_SendSerialToBrain );
sSerial = "";
}
}
vTaskDelete( NULL );
} // void fSendSerialToBrain( void *pvParameters )
//////////////////////////////////////////////
void fGetIMU( void *pvParameters )
{
// mpu9250.setQSize( 3 ); // set queue size to other than default of 1 for queued transactions
// change default before init.
mpu9250.fInit_MPU9250( false, false );
////
// const float alpha = 0.5;
boolean DidBias = false;
// boolean DidBias = true;
float TimePast = micros() / 1000000.0f;
float TimeNow = micros() / 1000000.0f;
// xSemaphoreGive( sema_GetIMU );
TickType_t xLastWakeTime = xTaskGetTickCount();
TickType_t xFrequency = 30;
//// first trigger comes from setup, when calibration complete enable interrupt
//// interrupts cause the rest of the event triggers.
for (;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
// xEventGroupWaitBits (eg, evtGetIMU, pdTRUE, pdTRUE, portMAX_DELAY);
// read the sensor
if ( DidBias )
{
//TimeNow = xTaskGetTickCount();
TimeNow = micros() / 1000000.0f;
deltat = ( TimeNow - TimePast);
////
// Serial.println ( deltat, 6 );
mpu9250.fGetAll();
////
// Serial.print ( "ax = " );
// Serial.print ( mpu9250.getAxScaled(), 6 );
// Serial.print ( ", " );
// Serial.print ( " ay = " );
// Serial.print ( mpu9250.getAyScaled(), 6 );
// Serial.print ( ", " );
// Serial.print ( " az = " );
// Serial.print ( mpu9250.getAzScaled(), 6 );
// Serial.print ( ", " );
// Serial.print ( " gx = " );
// Serial.print ( mpu9250.getGxScaled(), 6 );
// Serial.print ( mpu9250.getGxRaw(), 6 );
// Serial.print ( ", " );
// Serial.print ( " gy = " );
// Serial.print ( mpu9250.getGyScaled(), 6 );
// Serial.print ( ", " );
// Serial.print ( mpu9250.getGyRaw(), 6 );
// Serial.print ( ", " );
// Serial.print ( " gz = " );
// Serial.print ( mpu9250.getGzScaled(), 6 );
// Serial.print( " deg/s" );
// Serial.print ( " hx = " );
// Serial.print ( mpu9250.getMxScaled(), 6 );
// Serial.print ( ", " );
// Serial.print ( " hy = " );
// Serial.print ( mpu9250.getMyScaled(), 6 );
// Serial.print ( ", " );
// Serial.print ( " hz = " );
// Serial.print ( mpu9250.getMzScaled(), 6 );
// Serial.print ( ", " );
////Low Pass Filter
// fXg = Xg * alpha + (fXg * (1.0 - alpha));
// fYg = Yg * alpha + (fYg * (1.0 - alpha));
// fZg = Zg * alpha + (fZg * (1.0 - alpha));
//Roll & Pitch Equations from accelerometers only
// float roll2 = (atan2(-mpu9250.getAyScaled(), mpu9250.getAzScaled())*180.0)/M_PI;
// float pitch2 = (atan2( mpu9250.getAxScaled(), sqrt(mpu9250.getAyScaled()*mpu9250.getAyScaled() + mpu9250.getAzScaled()*mpu9250.getAzScaled()))*180.0)/M_PI;
//Roll & Pitch Equations
// Serial.print( " roll2 " );
// Serial.print( roll2 );
// Serial.print( ", " );
// Serial.print( " pitch2 " );
// Serial.print( pitch2 );
// Serial.println();
MahonyQuaternionUpdate ( mpu9250.getAxScaled(), mpu9250.getAyScaled(), mpu9250.getAzScaled(), mpu9250.getGxScaled() * PI / 180.0f, mpu9250.getGyScaled() * PI / 180.0f, mpu9250.getGzScaled() * PI / 180.0f, mpu9250.getMxScaled(), mpu9250.getMyScaled(), mpu9250.getMzScaled() );
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
Serial.print( roll );
Serial.print( ", " );
Serial.print( pitch );
// Serial.println( );
// pxXYZ.pitch *= 180.0f / PI;
// pxXYZ.pitch *= 90.0f / PI;
// pxXYZ.yaw *= 180.0f / PI;
// pxXYZ.yaw += 13.13f; // Declination
// pxXYZ.roll *= 180.0f / PI;
// pxXYZ.roll *= 90.0f / PI;
if ( xSemaphoreTake( sema_X, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
{
xQueueOverwrite( xQ_X_INFO, (void *) &roll );
xEventGroupSetBits( eg, evtTweakServoX );
}
if ( xSemaphoreTake( sema_Y, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
{
xQueueOverwrite( xQ_Y_INFO, (void *) &pitch );
xEventGroupSetBits( eg, evtTweakServoY );
}
TimePast = TimeNow;
} // if ( DidBias )
else
{
// MPU calibration
// fCalculateM_offset( );
// Serial.print( "calibration values ");
// Serial.print( ", ax ");
// Serial.print ( mpu9250.getAXbias(), 6 );
// Serial.print( ", ay ");
// Serial.print ( mpu9250.getAYbias(), 6 );
// Serial.print( ", az ");
// Serial.print ( mpu9250.getAZbias(), 6 );
// Serial.print( ", gx ");
// Serial.print ( mpu9250.getGXbias(), 6 );
// Serial.print( ", gy ");
// Serial.print ( mpu9250.getGYbias(), 6 );
// Serial.print( ", gz ");
// Serial.print ( mpu9250.getGZbias(), 6 );
// Serial.print( ", mx ");
// Serial.print ( mpu9250.getMXbias(), 6 );
// Serial.print( ", my ");
// Serial.print ( mpu9250.getMYbias(), 6 );
// Serial.print( ", mz ");
// Serial.print ( mpu9250.getMZbias(), 6 );
// Serial.println();
DidBias = true;
//// /* setting the interrupt */
// mpu9250.fSetInterrupt();
// pinMode( MPU_int_Pin, INPUT );
// attachInterrupt( MPU_int_Pin, triggerGet_IMU, RISING );
TimePast = micros();
xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange ); // start the LIDAR scanning
}
xLastWakeTime = xTaskGetTickCount();;
} // for (;;)
vTaskDelete( NULL );
} // void fGetIMU( void *pvParameters )
//////////////////////////////////////////////////////////////////////////////////
void fCalculateM_offset( )
{
int ServoZMax = iLIDAR_Posit90 + 250;
int ServoZMin = iLIDAR_Posit90 - 250;
int ServoYMax = iY_Posit90 + 250;
int ServoYMin = iY_Posit90 - 250;
int ServoXMax = iX_Posit90 + 350;
int ServoXMin = iX_Posit90 - 350;
//
int numOfSamples = 75;
int Xmax = -32767;
int Xmin = 32767;
int Ymax = -32767;
int Ymin = 32767;
int Zmax = -32767;
int Zmin = 32767;
TickType_t ServoDelay = pdMS_TO_TICKS( 300 );
TickType_t MeasurementDelay = pdMS_TO_TICKS( 20 );
///////////////////////////////////////////////
mpu9250.fCalculate_GandAbias();
//////////////////////////////////////////////
mpu9250.setMXbias( 0.0f );
mpu9250.setMYbias( 0.0f );
mpu9250.setMZbias( 0.0f );
/////// do magnemeter bias
// get centered reading
// rotate X servo to cause Z axis to swing to either end
// take initial readings
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
// Serial.println( mpu9250.getMagDataOverflow() ); // false is normal
}
// send X servo to one direction
ledcWrite( Channel_X, usToTicks ( ServoXMax ) );
vTaskDelay( ServoDelay );
// take a reading and average
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
}
// send X servo to other direction
ledcWrite( Channel_X, usToTicks ( ServoXMin ) );
vTaskDelay( ServoDelay );
// take a reading and average
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
}
// // x servo to middle position
ledcWrite( Channel_X, usToTicks ( iX_Posit90 ) );
vTaskDelay( ServoDelay );
// do Y servo
// send Y to one end
ledcWrite (Channel_Y, usToTicks ( ServoYMax ) );
vTaskDelay( ServoDelay );
// take a reading and average
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
}
// send Y servo to other end
ledcWrite (Channel_Y, usToTicks ( ServoYMin ) );
// take a reading
// take a reading and average
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
}
// put servo y back to starting posit
ledcWrite ( Channel_Y, usToTicks ( iY_Posit90 ) ); //
vTaskDelay( ServoDelay );
// do X
// send Z to one end
ledcWrite (Channel_LIDAR, usToTicks ( ServoZMax ) );
vTaskDelay( ServoDelay );
// take a reading and average
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
}
// send Z servo to other end
ledcWrite (Channel_LIDAR, usToTicks ( ServoZMin ) );
vTaskDelay( ServoDelay );
// take reading
// take a reading and average
for ( int i = 0; i < numOfSamples; i++)
{
mpu9250.fGetAll();
fDoMinMax_mag( Xmax, Xmin, (int)mpu9250.getMxRaw() );
fDoMinMax_mag( Ymax, Ymin, (int)mpu9250.getMyRaw() );
fDoMinMax_mag( Zmax, Zmin, (int)mpu9250.getMzRaw() );
vTaskDelay( MeasurementDelay );
}
// put servo Z back to starting posit
ledcWrite ( Channel_LIDAR, usToTicks ( iLIDAR_Posit90 ) ); //
// Get hard iron correction, set M bias values
mpu9250.setMXbias( ( Xmax + Xmin ) / 2 ); // _mb * mRes * x_MSF.x is done with set M bias
mpu9250.setMYbias( ( Ymax + Ymin ) / 2 );
mpu9250.setMZbias( ( Zmax + Zmin) / 2 );
// Get soft iron correction estimate
float Xmag_scale = ( Xmax - Xmin ) / 2; // get average x axis max chord length in counts
float Ymag_scale = ( Ymax - Ymin ) / 2; // get average y axis max chord length in counts
float Zmag_scale = ( Zmax - Zmin ) / 2; // get average z axis max chord length in counts
float avg_rad = ( Xmag_scale + Ymag_scale + Zmag_scale ) / 3.0f;
// set mag scale
mpu9250.setMxScale( avg_rad / Xmag_scale );
mpu9250.setMyScale( avg_rad / Ymag_scale );
mpu9250.setMzScale( avg_rad / Zmag_scale );
vTaskDelay( 100 );
// set gyro values to operating range
// mpu9250. setGyroRange( mpu9250.useGYRO_RANGE_2000DPS() );
// leaving gyro at
mpu9250.setDlpfBandwidthGyro( mpu9250.useDLPFbandwidth184Hz() );
// setting acceloremeter to
mpu9250.setAccelRange( mpu9250.useAccelerometerRange_8G() );
} //void fCalculateM_offset()
//////
void fDoMinMax_mag( int &_max, int &_min, int magValue)
{
if ( magValue > _max )
{
_max = magValue;
}
if ( magValue < _min )
{
_min = magValue;
}
} // void fDoMinMax_mag( int &_max, int &_min, int magValue)
//////
//// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
//// measured ones.
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
} // void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
//////////////////////////////////////////////
void fBlinkBuiltIn( void* pvParameters )
{
// toggle built in LED off/on
for (;;)
{
vTaskDelay( pdMS_TO_TICKS( 10 ) );
REG_WRITE( GPIO_OUT_W1TC_REG, BIT2 ); // GPIO2 LOW (clear)
vTaskDelay( pdMS_TO_TICKS( OneK ) );
REG_WRITE( GPIO_OUT_W1TS_REG, BIT2 ); //GPIO2 HIGH (set)
}
vTaskDelete( NULL );
}
////////******************************************
///* save below function */
////void getTFminiData(int* distance, int* strength)
////{
//// static char i = 0;
//// char j = 0;
//// int checksum = 0;
//// static int rx[9];
//// if (SerialTFMini.available()) {
//// rx[i] = SerialTFMini.read();
//// if (rx[0] != 0x59) {
//// i = 0;
//// } else if (i == 1 && rx[1] != 0x59) {
//// i = 0;
//// } else if (i == 8) {
//// for (j = 0; j < 8; j++) {
//// checksum += rx[j];
//// }
//// if (rx[8] == (checksum % 256)) {
//// *distance = rx[2] + rx[3] * 256;
//// *strength = rx[4] + rx[5] * 256;
//// }
//// i = 0;
//// } else {
//// i++;
//// }
//// }
////}
void fDoLIDAR( void * pvParameters )
{
int iLIDAR_Distance = 0;
//// int iLIDAR_Strength = 0;
// Channel_Five
while (1)
{
xEventGroupWaitBits (eg, evtDoLIDAR, pdTRUE, pdTRUE, portMAX_DELAY) ;
// Serial.println( "evtLIDAR_ServoAspectChangeb " );
tfmini.externalTrigger();
iLIDAR_Distance = tfmini.getDistance();
if ( iLIDAR_Distance > LIDAR_Max_Distance )
{
Serial.print ( " TFMini distance issue " );
Serial.println ( iLIDAR_Distance );
}
//// iLIDAR_Strength = tfmini.getRecentSignalStrength();
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
x_LIDAR_INFO.Range[x_LIDAR_INFO.CurrentCell] = iLIDAR_Distance;
xSemaphoreGive( sema_LIDAR_INFO );
// Serial.println ( iLIDAR_Distance );
xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange );
// Serial.print( "fDoLIDAR " );
// Serial.print(uxTaskGetStackHighWaterMark( NULL ));
// Serial.println();
// Serial.flush();
}
vTaskDelete( NULL );
}
//////******************************************
void fTweakServoX( void * parameter )
{
float temp;
TickType_t ServoDelay = pdMS_TO_TICKS( 30 ); // limits degrees per second
int Max = usToTicks( 1747 );
int Min = usToTicks( 1349 );
int CurrentPosition = iX_Posit90;
// int OneTick = usToTicks( 1 ); // 3 ticks per uSec
// rectified torque value
int rectifiedTorque = 0;
while (1)
{
xEventGroupWaitBits (eg, evtTweakServoX, pdTRUE, pdTRUE, portMAX_DELAY) ;
xQueueReceive ( xQ_X_INFO, &temp, QueueReceiveDelayTime );
// limit of 45 to 135 degrees
// Serial.print( (int)temp );
if ( (int)temp !=0 )
{
// Serial.print( " current Position: " );
// Serial.print( CurrentPosition + (int)temp );
// if ( abs(temp) < 45.1f )
// {
// temp *= 5.54f; // roll to servo torque uSeconds
// // sum with iX_Posit90
// temp = iX_Posit90 + temp;
// //convert to clock ticks
// // torque servo
// // rectifiedTorque = ( rectifiedTorque + int(temp) ) / 2;
//
// xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
// ledcWrite( Channel_X, usToTicks((int)temp) );
// ledcWrite( Channel_X, usToTicks(CurrentPosition) );
// Serial.print( temp );
// Serial.print( ", " );
// Serial.print( rectifiedTorque );
// Serial.println();
// }
// else
// {
// xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
// ledcWrite( Channel_X, usToTicks(iX_Posit90) );
}
xSemaphoreGive ( sema_LIDAR );
vTaskDelay( ServoDelay );
xSemaphoreGive( sema_X );
Serial.println();
}
vTaskDelete( NULL );
}
void fTweakServoY( void * parameter )
{
float temp;
TickType_t ServoDelay = pdMS_TO_TICKS( 30 ); // limits degrees per second
int Max = usToTicks( 1747 );
int Min = usToTicks( 1349 );
// int OneTick = usToTicks( 1 ); // 3 ticks per uSec
// rectified torque value
// int rectifiedTorque = 0;
while (1)
{
xEventGroupWaitBits (eg, evtTweakServoY, pdTRUE, pdTRUE, portMAX_DELAY) ;
xQueueReceive ( xQ_Y_INFO, &temp, QueueReceiveDelayTime );
// Serial.print( temp );
// Serial.println();
// if ( abs(temp) < 45.1f )
// {
temp *= 5.54f; // roll to servo torque uSeconds
// sum with iY_Posit90
temp = iY_Posit90 + temp;
//convert to clock ticks
// torque servo
// rectifiedTorque = ( rectifiedTorque + int(temp) ) / 2; // rectify torque
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite (Channel_Y, usToTicks( (int)temp) );
// ledcWrite (Channel_Y, usToTicks( rectifiedTorque) );
// Serial.print( temp,6 );
// Serial.print( ", " );
// Serial.print( rectifiedTorque );
// Serial.println();
// }
// else
// {
// xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
// ledcWrite (Channel_Y, usToTicks(iY_Posit90) );
// }
xSemaphoreGive ( sema_LIDAR );
vTaskDelay( ServoDelay );
xSemaphoreGive( sema_Y );
}
vTaskDelete( NULL );
}
////
//////****************************************************
void fLIDAR_ServoAspectChange( void * parameter )
{
// struct stu_XYZ_INFO pxXYZ;
// screen size 64, doing 62 scans for the range
int ServoMax = 1537;
int ServoMin = 1413;
int ServoIncrement = 2;
int ServoCurrent = 0;
// TickType_t ServoDelay = pdMS_TO_TICKS( 12 );
while (1)
{
xEventGroupWaitBits (eg, evtLIDAR_ServoAspectChange, pdTRUE, pdTRUE, portMAX_DELAY) ;
if ( ServoCurrent == 0 )
{
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite( Channel_LIDAR, usToTicks ( ServoMin ) );
xSemaphoreGive ( sema_LIDAR );
ServoCurrent = ServoMin;
x_LIDAR_INFO.CurrentCell = 1;
x_LIDAR_INFO.ServoSweepUp = true;
vTaskDelay( pdMS_TO_TICKS( ServoDelay40mS ) );
}
else
{
if ( x_LIDAR_INFO.ServoSweepUp )
{
ServoCurrent += ServoIncrement; // set next servo position to torque to
x_LIDAR_INFO.CurrentCell += 1;
// Serial.println ( ServoCurrent );
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite( Channel_LIDAR, usToTicks ( ServoCurrent ) );
xSemaphoreGive ( sema_LIDAR );
if ( ServoCurrent >= ServoMax )
{
x_LIDAR_INFO.ServoSweepUp = false; // toggle servo direction
//
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
xQueueOverwrite( xQ_LIDAR_INFO, (void *) &x_LIDAR_INFO );
xSemaphoreGive ( sema_LIDAR_INFO );
//
xSemaphoreTake( sema_LIDAR_FOR_ALARM, xSemaphoreTicksToWait );
xQueueOverwrite ( xQ_LIDAR_FOR_ALARM, (void *) &x_LIDAR_INFO );
xSemaphoreGive( sema_LIDAR_FOR_ALARM );
//
// Serial.println ( " lidar sweep end send serial to brain trigger" );
xEventGroupSetBits( eg, evtSendSerialToBrain );
}
} // if ( ServoSweepUp )
else // servo sweep down
{
ServoCurrent -= ServoIncrement;
x_LIDAR_INFO.CurrentCell -= 1; // set next cell position
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite( Channel_LIDAR, usToTicks ( ServoCurrent ) );
xSemaphoreGive ( sema_LIDAR );
// has sweep completed
if ( ServoCurrent <= ServoMin )
{
x_LIDAR_INFO.ServoSweepUp = true; // toggle servo direction
//
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
xQueueOverwrite( xQ_LIDAR_INFO, (void *) &x_LIDAR_INFO );
xSemaphoreGive ( sema_LIDAR_INFO );
//
xSemaphoreTake( sema_LIDAR_FOR_ALARM, xSemaphoreTicksToWait );
xQueueOverwrite ( xQ_LIDAR_FOR_ALARM, (void *) &x_LIDAR_INFO );
xSemaphoreGive( sema_LIDAR_FOR_ALARM );
//
xEventGroupSetBits( eg, evtSendSerialToBrain );
} // if ( ServoCurrent <= ServoMin )
} // else // servo sweep down
} //
vTaskDelay( ServoDelay12mS );
xEventGroupSetBits( eg, evtDoLIDAR );
// Serial.print( "fLIDAR_ServoAspectChange " );
// Serial.print(uxTaskGetStackHighWaterMark( NULL ));
// Serial.println();
// Serial.flush();
}
vTaskDelete( NULL );
}
I no longer maintain that project. Happy studying.