May I suggest you actually build a test setup to verify your proposal. When your colimated laser light encounters your curved surface, you will get all kinds of distortions. There will not be a nice clean "shadow " as you expect.
Paul
1 Like
Good point, this is something I was thinking about, I will replace the LDR with photo diode for better response and top of that my sketch will include to look for peak amplitude of photodiode's response and will discard any responses below the peak amplitude. And I can adjust the sketch to look for the optimal amplitude peak of the photo diode.
I used a LIDAR to measure the OD of a pipe. I did not do it to get a measurement, I did it to get an image. I had a LIDAR mounted on a platform that scanned side to side. I placed the pipe at so many inches away against a wall. The LIDAR scanned the wall and pipe. I then used the distance measurements to recreate the 'image' of a pipe against a wall. The pipe was 72CM from the LIDAR focal point. I was able to create an image on a OLED display showing the pipe against the wall.
Idahowalker, that is great. I have been reading on ToF sensors and thinking about using them instead. can you give me full detail of how you did that please? I would be curiously interested on how you putogether the sketch. I look forward to see your work. Thank you.
My work. Simple Put a LIDAR on top of a platform that rotates. I then used a metal geared servo to rotate the servo. While the servo rotates in between rotations the LIDAR fires to the wall, which was at a measured distance away, get the distance, subtract from expected wall distance to get deviations display on OLED screen.
Do you have a webkink to your project, that I can study it?
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.
The OP may be interested in how some pipe manufacturers measure their products in the production line. Here's one method, by Starrett (a well-known name in measurement technology)
I wouldn't be surprised if this unit costs US$50,000 or more, not including installation and training.
Something similar could probably be done at a hobby level with an RPi, but it would be quite a challenge.
Some quotes:
"...the measurement work envelope is from a minimum diameter of 125mm (5”) to a maximum diameter of 600mm (24”)...."
"...accuracy is 0.15mm on the diameter and is verified by checking a metrology lab-certified gauge block with known dimensions..."
"...sensors are mounted at four positions 90 degrees apart..."
"...each sensor projects a laser line across the pipe diameter. A camera in the sensor takes a snapshot of the laser line at 45 degree angle. This exposes the laser line
as an arc. The CCD array in the camera digitizes the arc into 1,600 columns of
pixels. Each column has 1,200 pixels...."
"...software converts the pixel data into spatial x-y coordinates....the coordinates from 4 sensors are combined into a single data set using a patented registration algorithm. The resulting point cloud is analyzed using mathematical tools that calculate various measurement parameters for each cross section...."
Wow, that's an impressive setup. Wonder if I can achieve something like this on Arduino DIY scale at affordable college budget. I can immediately think to use the scanner linear CCD sensor with Arduino laser may be.
That's most helpful. I am studying your code at present and understand your approach to mapping. Will come back with questions.
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.