commit
582860d2ab
@ -0,0 +1,5 @@
|
||||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
@ -0,0 +1,10 @@
|
||||
{
|
||||
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||
// for the documentation about the extensions.json format
|
||||
"recommendations": [
|
||||
"platformio.platformio-ide"
|
||||
],
|
||||
"unwantedRecommendations": [
|
||||
"ms-vscode.cpptools-extension-pack"
|
||||
]
|
||||
}
|
||||
@ -0,0 +1,40 @@
|
||||
#include <vector>
|
||||
#include "WS2812Manager.hpp"
|
||||
|
||||
class HellMetServo{
|
||||
private:
|
||||
int _origin = 0;
|
||||
int _movement = 0;
|
||||
int _pin = -1;
|
||||
int _start = 0;
|
||||
float lastAngle = 0;
|
||||
float _target = 0;
|
||||
volatile float _angle = 0;
|
||||
volatile int servoHandle = 0;
|
||||
bool wasClosing = false;
|
||||
public:
|
||||
HellMetServo(int origin, int movement, int pin);
|
||||
void StartToggle();
|
||||
void StartOpen();
|
||||
void StartClose();
|
||||
bool Step();
|
||||
void Disable();
|
||||
void Enable();
|
||||
int getCurrentAngle(){return _angle;}
|
||||
int getCurrentTarget(){return _target;}
|
||||
int getPin(){return _pin;}
|
||||
int getHandle(){return servoHandle;}
|
||||
};
|
||||
|
||||
class HellMetServoManager{
|
||||
private:
|
||||
std::vector<HellMetServo*> topServos = std::vector<HellMetServo*>();
|
||||
std::vector<HellMetServo*> bottomServos = std::vector<HellMetServo*>();
|
||||
std::vector<HellMetServo*> allServos = std::vector<HellMetServo*>();
|
||||
int timingsArray[11] = {20,15,7,1,1,1,1,7,15,20,20};
|
||||
void ToggleAll();
|
||||
public:
|
||||
volatile bool in_use = false;
|
||||
HellMetServoManager();
|
||||
static void ToggleAllWrapper(void *classInstance);
|
||||
};
|
||||
@ -0,0 +1,29 @@
|
||||
#include <wire.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
class I2CDevice{
|
||||
protected:
|
||||
int _writeAddress = 0x00;
|
||||
int _readAddress = 0xFF;
|
||||
std::string _name = "";
|
||||
public:
|
||||
int getWriteAddress(){return _writeAddress;}
|
||||
int getReadAddress(){return _readAddress;}
|
||||
std::string getname(){return _name;}
|
||||
virtual bool checkAlive(){return false;}
|
||||
};
|
||||
|
||||
class I2CManager{
|
||||
protected:
|
||||
static std::vector<I2CDevice*> _Devices;
|
||||
static int _pinSCL;
|
||||
static int _pinSDA;
|
||||
public:
|
||||
static void Setup(int pinSDA, int pinSCL);
|
||||
static void I2CWrite(int address, int value);
|
||||
static int I2CRead(int address, int size);
|
||||
static void writeToRegister8bit(int message, int targetRegister, int i2cAddress);
|
||||
static void registerNewDevice(I2CDevice* device){_Devices.push_back(device);}
|
||||
static void scanAll();
|
||||
};
|
||||
@ -0,0 +1,26 @@
|
||||
#include <Arduino.h>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/task.h>
|
||||
#include <freertos/semphr.h>
|
||||
|
||||
extern SemaphoreHandle_t serial_mutex;
|
||||
|
||||
extern void THREAD_SAFE_PRINTLN(String data);
|
||||
|
||||
enum debugLevel{
|
||||
none = 0,
|
||||
error = 1,
|
||||
warn = 2,
|
||||
info = 3,
|
||||
verbose = 4 //only use as a last resort, it makes things go real bad on the serial port
|
||||
};
|
||||
|
||||
#define DEBUGLEVEL debugLevel::info
|
||||
|
||||
#define LOG_ERROR(x) do { if (DEBUGLEVEL >= debugLevel::error) { THREAD_SAFE_PRINTLN(x); } } while (0)
|
||||
|
||||
#define LOG_WARN(x) do { if (DEBUGLEVEL >= debugLevel::warn) { THREAD_SAFE_PRINTLN(x); } } while (0)
|
||||
|
||||
#define LOG_INFO(x) do { if (DEBUGLEVEL >= debugLevel::info) { THREAD_SAFE_PRINTLN(x); }} while (0)
|
||||
|
||||
#define LOG_VERBOSE(x) do { if (DEBUGLEVEL >= debugLevel::verbose) { THREAD_SAFE_PRINTLN(x); }} while (0)
|
||||
@ -0,0 +1,39 @@
|
||||
|
||||
This directory is intended for project header files.
|
||||
|
||||
A header file is a file containing C declarations and macro definitions
|
||||
to be shared between several project source files. You request the use of a
|
||||
header file in your project source file (C, C++, etc) located in `src` folder
|
||||
by including it, with the C preprocessing directive `#include'.
|
||||
|
||||
```src/main.c
|
||||
|
||||
#include "header.h"
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Including a header file produces the same results as copying the header file
|
||||
into each source file that needs it. Such copying would be time-consuming
|
||||
and error-prone. With a header file, the related declarations appear
|
||||
in only one place. If they need to be changed, they can be changed in one
|
||||
place, and programs that include the header file will automatically use the
|
||||
new version when next recompiled. The header file eliminates the labor of
|
||||
finding and changing all the copies as well as the risk that a failure to
|
||||
find one copy will result in inconsistencies within a program.
|
||||
|
||||
In C, the usual convention is to give header files names that end with `.h'.
|
||||
It is most portable to use only letters, digits, dashes, and underscores in
|
||||
header file names, and at most one dot.
|
||||
|
||||
Read more about using header files in official GCC documentation:
|
||||
|
||||
* Include Syntax
|
||||
* Include Operation
|
||||
* Once-Only Headers
|
||||
* Computed Includes
|
||||
|
||||
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||
@ -0,0 +1,15 @@
|
||||
#include<Freenove_WS2812_Lib_for_ESP32.h>
|
||||
|
||||
class WS2812Manager{
|
||||
private:
|
||||
Freenove_ESP32_WS2812* strip;
|
||||
int _ledCount;
|
||||
void RGBCycleAll(int tempo);
|
||||
void setAllColor(int R, int G, int B);
|
||||
void BreatheOutAllColor(int R, int G, int B, int tempo);
|
||||
void BreatheInAllColor(int R, int G, int B, int tempo);
|
||||
void BreatheAllColor(int R, int G, int B, int tempo);
|
||||
public:
|
||||
WS2812Manager(int ledPin, int ledCount);
|
||||
static void RunRGB(void *classInstance);
|
||||
};
|
||||
@ -0,0 +1,24 @@
|
||||
#include "I2CManagement.hpp"
|
||||
#define X 0
|
||||
#define Y 1
|
||||
#define Z 2
|
||||
|
||||
class Hmc5883L : I2CDevice{
|
||||
protected:
|
||||
int16_t values[3] = {0,0,0};
|
||||
double valuesScaled[3] = {0,0,0};
|
||||
double radius = 0;
|
||||
double azimuth = 0;
|
||||
double elevation = 0;
|
||||
double heading = 0;
|
||||
void readRawValues();
|
||||
public:
|
||||
Hmc5883L(int readAddress, int writeAddress);
|
||||
void beginUpdateTask();
|
||||
void update();
|
||||
static void beginUpdateTaskWrapper(void *classInstance);
|
||||
bool checkAlive() override {return true;}
|
||||
String getRawValuesWithSeparator();
|
||||
String getHeadingValue(){return String(heading);}
|
||||
String getDebugValues();
|
||||
};
|
||||
@ -0,0 +1 @@
|
||||
#include "HellMetServo.hpp"
|
||||
@ -0,0 +1,46 @@
|
||||
|
||||
This directory is intended for project specific (private) libraries.
|
||||
PlatformIO will compile them to static libraries and link into executable file.
|
||||
|
||||
The source code of each library should be placed in a an own separate directory
|
||||
("lib/your_library_name/[here are source files]").
|
||||
|
||||
For example, see a structure of the following two libraries `Foo` and `Bar`:
|
||||
|
||||
|--lib
|
||||
| |
|
||||
| |--Bar
|
||||
| | |--docs
|
||||
| | |--examples
|
||||
| | |--src
|
||||
| | |- Bar.c
|
||||
| | |- Bar.h
|
||||
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||
| |
|
||||
| |--Foo
|
||||
| | |- Foo.c
|
||||
| | |- Foo.h
|
||||
| |
|
||||
| |- README --> THIS FILE
|
||||
|
|
||||
|- platformio.ini
|
||||
|--src
|
||||
|- main.c
|
||||
|
||||
and a contents of `src/main.c`:
|
||||
```
|
||||
#include <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
PlatformIO Library Dependency Finder will find automatically dependent
|
||||
libraries scanning project source files.
|
||||
|
||||
More information about PlatformIO Library Dependency Finder
|
||||
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||
@ -0,0 +1,25 @@
|
||||
; PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:esp32-c3-devkitm-1]
|
||||
platform = espressif32
|
||||
board = esp32-c3-devkitm-1
|
||||
framework = arduino
|
||||
;board_upload.flash_size=2MB
|
||||
lib_deps =
|
||||
khoih-prog/ESP32_C3_ISR_Servo@^1.2.0
|
||||
freenove/Freenove WS2812 Lib for ESP32@^1.0.5
|
||||
monitor_speed = 460800
|
||||
build_flags =
|
||||
-D ARDUINO_USB_MODE=1
|
||||
-D ARDUINO_USB_CDC_ON_BOOT=1
|
||||
-D SERIALCONS=USBSerial
|
||||
;board_build.filesystem = littlefs
|
||||
;board_build.partitions = minimal.csv
|
||||
@ -0,0 +1,116 @@
|
||||
#include "HellMetServo.hpp"
|
||||
#include "PinDefinitions.hpp"
|
||||
|
||||
#define USE_ESP32_TIMER_NO 1
|
||||
#define TIMER_INTERRUPT_DEBUG 0
|
||||
#define ISR_SERVO_DEBUG 0
|
||||
#define MIN_MICROS 800
|
||||
#define MAX_MICROS 2450
|
||||
#define ADDITIONNAL_MOVEMENT_OFFSET 25
|
||||
|
||||
#include <ESP32_C3_ISR_Servo.h>
|
||||
|
||||
HellMetServo::HellMetServo(int origin, int movement, int pin) : _origin(origin), _movement(movement), _pin(pin){
|
||||
LOG_INFO("Creating servo on pin" + String(pin));
|
||||
servoHandle = ESP32_ISR_Servos.setupServo(pin, MIN_MICROS, MAX_MICROS);
|
||||
LOG_INFO("Servo Created with handle" + String(servoHandle));
|
||||
lastAngle = origin;
|
||||
_angle = origin;
|
||||
}
|
||||
|
||||
void HellMetServo::StartToggle(){
|
||||
wasClosing ? StartOpen() : StartClose();
|
||||
}
|
||||
|
||||
void HellMetServo::StartClose(){
|
||||
_start = _origin-_movement;
|
||||
_target = _origin;
|
||||
_angle = _start;
|
||||
wasClosing = true;
|
||||
}
|
||||
|
||||
void HellMetServo::StartOpen(){
|
||||
_start = _origin;
|
||||
_target = _origin-_movement;
|
||||
_angle = _start;
|
||||
wasClosing = false;
|
||||
}
|
||||
|
||||
bool HellMetServo::Step(){
|
||||
if(_angle == _target) return true;
|
||||
float completion = abs(_target - lastAngle);
|
||||
_angle += (_angle > _target) ? -1 : 1;
|
||||
lastAngle = _angle;
|
||||
if(_angle > 180 || _angle < 0 || !ESP32_ISR_Servos.setPosition(servoHandle, _angle)){
|
||||
LOG_ERROR("Error moving servo "+String(_pin)+" to angle "+_angle);
|
||||
return true; //stop
|
||||
}
|
||||
return _angle == _target;
|
||||
}
|
||||
|
||||
void HellMetServo::Disable(){
|
||||
if(!ESP32_ISR_Servos.disable(servoHandle))
|
||||
LOG_ERROR("Error disableing servo "+String(_pin));
|
||||
}
|
||||
|
||||
void HellMetServo::Enable(){
|
||||
if(!ESP32_ISR_Servos.enable(servoHandle))
|
||||
LOG_ERROR("Error enabling servo "+String(_pin));
|
||||
}
|
||||
|
||||
//HellMetServoManager defintions ---------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
HellMetServoManager::HellMetServoManager(){
|
||||
ESP32_ISR_Servos.useTimer(USE_ESP32_TIMER_NO);
|
||||
|
||||
allServos.push_back(new HellMetServo(90+3+ADDITIONNAL_MOVEMENT_OFFSET, (50+ADDITIONNAL_MOVEMENT_OFFSET), GPIO_NUM_3)); //Top right
|
||||
allServos.push_back(new HellMetServo(78+0+6+ADDITIONNAL_MOVEMENT_OFFSET, (50+ADDITIONNAL_MOVEMENT_OFFSET), GPIO_NUM_2)); //bottom right
|
||||
allServos.push_back(new HellMetServo(28+4+1-ADDITIONNAL_MOVEMENT_OFFSET, -(50+ADDITIONNAL_MOVEMENT_OFFSET), GPIO_NUM_4)); //bottom left
|
||||
allServos.push_back(new HellMetServo(28+13-ADDITIONNAL_MOVEMENT_OFFSET, -(50+ADDITIONNAL_MOVEMENT_OFFSET), GPIO_NUM_5)); //top left
|
||||
//vTaskDelay(100);
|
||||
}
|
||||
|
||||
void HellMetServoManager::ToggleAll(){
|
||||
LOG_INFO("Beginning Toggle for "+String(allServos.size())+" servos");
|
||||
for (HellMetServo* servo : allServos)
|
||||
{
|
||||
LOG_INFO("Enabling");
|
||||
servo->Enable();
|
||||
LOG_INFO("Toggling");
|
||||
servo->StartToggle();
|
||||
}
|
||||
LOG_INFO("Done enabling servos");
|
||||
bool finished = false;
|
||||
int i = 0;
|
||||
while(!finished){
|
||||
finished = true;
|
||||
for(HellMetServo* servo : allServos){
|
||||
i++;
|
||||
LOG_WARN("not finished for servo "+String(servo->getHandle())+" angle: "+String(servo->getCurrentAngle())+" target: "+String(servo->getCurrentTarget()));
|
||||
if(!servo->Step()){
|
||||
LOG_INFO("Servo returned false "+String(i));
|
||||
finished = false;
|
||||
}
|
||||
LOG_INFO("Servo didn't return false "+String(i));
|
||||
}
|
||||
vTaskDelay(10/ portTICK_RATE_MS);
|
||||
}
|
||||
LOG_INFO("Done moving servos");
|
||||
vTaskDelay(100/ portTICK_RATE_MS);
|
||||
|
||||
for (HellMetServo* servo : allServos)
|
||||
{
|
||||
servo->Disable();
|
||||
}
|
||||
LOG_INFO("Toggle done");
|
||||
}
|
||||
|
||||
void HellMetServoManager::ToggleAllWrapper(void *classInstance){
|
||||
// Get a pointer to the object instance
|
||||
HellMetServoManager *obj = static_cast<HellMetServoManager *>(classInstance);
|
||||
obj->in_use = true;
|
||||
// Call the object method
|
||||
obj->ToggleAll();
|
||||
obj->in_use = false;
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
@ -0,0 +1,54 @@
|
||||
#include "I2CManagement.hpp"
|
||||
#include <arduino.h>
|
||||
#include "PinDefinitions.hpp"
|
||||
|
||||
std::vector<I2CDevice*> I2CManager::_Devices = std::vector<I2CDevice*>();
|
||||
int I2CManager::_pinSCL;
|
||||
int I2CManager::_pinSDA;
|
||||
|
||||
void I2CManager::Setup(int pinSDA, int pinSCL){
|
||||
_pinSCL = pinSCL;
|
||||
_pinSDA = pinSDA;
|
||||
Wire.setPins(_pinSDA, _pinSCL);
|
||||
Wire.begin();
|
||||
}
|
||||
|
||||
int I2CManager::I2CRead(int address, int size){
|
||||
return Wire.requestFrom(address, size);
|
||||
}
|
||||
|
||||
void I2CManager::I2CWrite(int address, int value){
|
||||
Wire.beginTransmission(address);
|
||||
Wire.write(value);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
void I2CManager::writeToRegister8bit(int message, int targetRegister, int i2cAddress){
|
||||
Wire.beginTransmission(i2cAddress); // Begins a transmission to the I2C slave
|
||||
Serial.print(Wire.write(targetRegister)); // ask to write to this register
|
||||
Serial.print(Wire.write(message)); // set this register's value to the message value (byte)
|
||||
Wire.endTransmission(true);
|
||||
}
|
||||
|
||||
void I2CManager::scanAll(){
|
||||
Serial.println ();
|
||||
Serial.println ("I2C scanner. Scanning ...");
|
||||
uint8_t count = 0;
|
||||
|
||||
for (uint8_t i = 8; i < 120; i++)
|
||||
{
|
||||
Wire.beginTransmission (i); // Begin I2C transmission Address (i)
|
||||
if (Wire.endTransmission () == 0) // Receive 0 = success (ACK response)
|
||||
{
|
||||
Serial.print ("Found address: ");
|
||||
Serial.print (i, DEC);
|
||||
Serial.print (" (0x");
|
||||
Serial.print (i, HEX); // PCF8574 7 bit address
|
||||
Serial.println (")");
|
||||
count++;
|
||||
}
|
||||
}
|
||||
Serial.print ("Found ");
|
||||
Serial.print (count, DEC); // numbers of devices
|
||||
Serial.println (" device(s).");
|
||||
}
|
||||
@ -0,0 +1,14 @@
|
||||
#include "PinDefinitions.hpp"
|
||||
|
||||
void THREAD_SAFE_PRINTLN(String data)
|
||||
{
|
||||
|
||||
// Wait for the mutex to be available
|
||||
xSemaphoreTake(serial_mutex, 10);
|
||||
|
||||
// Use the Serial class to send and receive data
|
||||
Serial.println(data);
|
||||
|
||||
// Release the mutex
|
||||
xSemaphoreGive(serial_mutex);
|
||||
}
|
||||
@ -0,0 +1,75 @@
|
||||
#include "WS2812Manager.hpp"
|
||||
#include "PinDefinitions.hpp"
|
||||
|
||||
WS2812Manager::WS2812Manager(int ledPin, int ledCount){
|
||||
strip = new Freenove_ESP32_WS2812(ledCount,ledPin);
|
||||
_ledCount = ledCount;
|
||||
if(!strip->begin())
|
||||
LOG_ERROR("WS2812Manager() error");
|
||||
}
|
||||
|
||||
void WS2812Manager::RGBCycleAll(int tempo){
|
||||
strip->setBrightness(255);
|
||||
for (int j = 0; j < 255; j++) { //cycle to blue
|
||||
for (int i = 0; i < _ledCount; i++) {
|
||||
strip->setLedColorData(i, strip->Wheel((i * 256 / _ledCount + j) & 255));
|
||||
}
|
||||
LOG_VERBOSE("RGBCycleAll to blue");
|
||||
strip->show();
|
||||
vTaskDelay(tempo/ portTICK_RATE_MS);
|
||||
}
|
||||
for (int j = 255; j > 0; j--) { //cycle back to red
|
||||
for (int i = 0; i < _ledCount; i++) {
|
||||
strip->setLedColorData(i, strip->Wheel((i * 256 / _ledCount + j) & 255));
|
||||
}
|
||||
LOG_VERBOSE("RGBCycleAll to red");
|
||||
strip->show();
|
||||
vTaskDelay(tempo/ portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
void WS2812Manager::setAllColor(int R, int G, int B){
|
||||
LOG_VERBOSE(String("setAllColor ")+String(R)+String(G)+String(B));
|
||||
esp_err_t error = strip->setAllLedsColor(R,G,B);
|
||||
if(error != 0){
|
||||
LOG_VERBOSE("setAllColor ERROR "+String(error));
|
||||
}
|
||||
//strip->show();
|
||||
}
|
||||
|
||||
void WS2812Manager::BreatheInAllColor(int R, int G, int B, int tempo){
|
||||
LOG_VERBOSE("BreatheInAllColor");
|
||||
LOG_VERBOSE("BreatheInAllColor set color done");
|
||||
setAllColor(R,G,B);
|
||||
for(int i = 0; i < 255; i++){
|
||||
strip->setBrightness(i);
|
||||
LOG_VERBOSE("BreatheInAllColor " + String(i)
|
||||
+' '+String(R) + ":" +String(G) + ":"+ String(B));
|
||||
vTaskDelay(tempo/ portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
void WS2812Manager::BreatheOutAllColor(int R, int G, int B, int tempo){
|
||||
LOG_INFO("BreatheOutAllColor");
|
||||
LOG_INFO("BreatheOutAllColor set color done");
|
||||
for(int i = 254; i > 0; i--){
|
||||
setAllColor(R,G,B);
|
||||
strip->setBrightness(i);
|
||||
LOG_INFO("BreatheOutAllColor " + String(i)
|
||||
+' '+String(R) + ":" +String(G) + ":"+ String(B));
|
||||
vTaskDelay(tempo/ portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
void WS2812Manager::BreatheAllColor(int R, int G, int B, int tempo){
|
||||
BreatheInAllColor(R,B,G,tempo);
|
||||
BreatheOutAllColor(R,G,B,tempo);
|
||||
}
|
||||
|
||||
void WS2812Manager::RunRGB(void *classInstance){
|
||||
// Get a pointer to the object instance
|
||||
WS2812Manager *obj = static_cast<WS2812Manager *>(classInstance);
|
||||
while(true){
|
||||
obj->RGBCycleAll(10);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,67 @@
|
||||
#include "hmc5883L.hpp"
|
||||
#include <math.h>
|
||||
#include "PinDefinitions.hpp"
|
||||
|
||||
#define _x valuesScaled[X]
|
||||
#define _y valuesScaled[Y]
|
||||
#define _z valuesScaled[Z]
|
||||
|
||||
Hmc5883L::Hmc5883L(int readAddress = 0x1E, int writeAddress = 0x1E){
|
||||
_readAddress = readAddress;
|
||||
_writeAddress = writeAddress;
|
||||
LOG_INFO("Setting up Hmc5883L");
|
||||
I2CManager::writeToRegister8bit(0b01110000, 0x00 , writeAddress ); //Configuration Register A
|
||||
I2CManager::writeToRegister8bit(0b11100000, 0x01 , writeAddress ); //Configuration Register B
|
||||
I2CManager::writeToRegister8bit(0b00000000, 0x02 , writeAddress ); //Mode register continuous measurement mode
|
||||
LOG_INFO("Done setting up Hmc5883L");
|
||||
}
|
||||
|
||||
void Hmc5883L::beginUpdateTask(){
|
||||
while(true){
|
||||
vTaskDelay(100/ portTICK_RATE_MS);
|
||||
LOG_VERBOSE("reading raw values");
|
||||
readRawValues();
|
||||
}
|
||||
}
|
||||
|
||||
void Hmc5883L::beginUpdateTaskWrapper(void *classInstance){
|
||||
LOG_INFO("beginUpdateTaskWrapper");
|
||||
Hmc5883L *obj = static_cast<Hmc5883L *>(classInstance);
|
||||
obj->beginUpdateTask();
|
||||
}
|
||||
|
||||
void Hmc5883L::readRawValues(){
|
||||
LOG_VERBOSE("Obtaining raw data");
|
||||
Wire.beginTransmission(_readAddress);
|
||||
Wire.write(0x03); // starting with register 0x03 (Data Output X MSB Register) [HMC5883L datasheet page 11]
|
||||
Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
|
||||
Wire.requestFrom(_readAddress, 6, 1); // request a total of 3*2=6 registers, and send the stop message
|
||||
LOG_VERBOSE("Obtained raw data");
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
values[i] = Wire.read() << 8 | Wire.read(); //we read the two registers at the same time, and concat them to get the full uint16_t (2*8)
|
||||
valuesScaled[i] = ((double)values[i]);
|
||||
}
|
||||
LOG_VERBOSE("Concatenated and scaled the data");
|
||||
radius = sqrt(pow(valuesScaled[X],2) + pow(valuesScaled[Y],2) + pow(valuesScaled[Z],2));
|
||||
azimuth = atan2(valuesScaled[Y], valuesScaled[X]);
|
||||
elevation = atan2(valuesScaled[Z], sqrt(pow(valuesScaled[X], 2) + pow(valuesScaled[Y], 2)));
|
||||
heading = atan2(values[Y], values[X]) * 180 / M_PI;
|
||||
LOG_VERBOSE("Recovered heading: "+String(heading));
|
||||
double head = 0;
|
||||
if (_x > 0) {
|
||||
heading = 90 - std::atan(_z/_x) * 180/M_PI;
|
||||
} else if (_x < 0) {
|
||||
heading = 270 - std::atan(_z/_x) * 180/M_PI;
|
||||
} else {
|
||||
if (_z < 0) {
|
||||
heading = 180.0;
|
||||
} else {
|
||||
heading = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
String Hmc5883L::getDebugValues(){
|
||||
return "X:" + String(_x) + "\nY:" + String(_y) +"\nZ:"+ String(_z);
|
||||
}
|
||||
@ -0,0 +1,73 @@
|
||||
#include "main.hpp"
|
||||
#include "PinDefinitions.hpp"
|
||||
#include "hmc5883L.hpp"
|
||||
|
||||
#if !defined( ARDUINO_ESP32C3_DEV )
|
||||
#error This code is intended to run on the ESP32_C3 platform! Please check your Tools->Board setting.
|
||||
#endif
|
||||
|
||||
HellMetServoManager* hm;
|
||||
WS2812Manager* LedManager;
|
||||
Hmc5883L* magnetometer;
|
||||
SemaphoreHandle_t serial_mutex = NULL;
|
||||
|
||||
void rtosLoop(void *pvParameter){
|
||||
LOG_VERBOSE("rtosloop");
|
||||
|
||||
if (Serial.available()){
|
||||
String str = Serial.readString();
|
||||
LOG_INFO(str);
|
||||
switch(str[0]){
|
||||
case 's': //servo
|
||||
switch(str[1]){
|
||||
case 't': //toggle All
|
||||
if(!hm->in_use)
|
||||
xTaskCreate(HellMetServoManager::ToggleAllWrapper, "Servo Thread", 4096, hm, 6, NULL);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case 'l': //leds
|
||||
break;
|
||||
case 'm' : //magnetometer
|
||||
switch(str[1]){
|
||||
case 'r': //read heading
|
||||
Serial.println(magnetometer->getHeadingValue());
|
||||
break;
|
||||
case 'd': //read debug
|
||||
Serial.println(magnetometer->getDebugValues());
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
LOG_INFO("rtosloop - serial present");
|
||||
Serial.flush();
|
||||
}
|
||||
LOG_VERBOSE("rtosloop - out");
|
||||
}
|
||||
|
||||
void setup() {
|
||||
serial_mutex = xSemaphoreCreateBinary();
|
||||
Serial.begin(9600);
|
||||
I2CManager::Setup(GPIO_NUM_9, GPIO_NUM_8);
|
||||
Serial.println("Are you alive?");
|
||||
while (!Serial.available());
|
||||
I2CManager::scanAll();
|
||||
magnetometer = new Hmc5883L(0x1E,0x1E);
|
||||
LOG_INFO("Creating mag thread");
|
||||
xTaskCreate(Hmc5883L::beginUpdateTaskWrapper, "magnetometer thread", 4096, magnetometer, 7, NULL);
|
||||
LOG_INFO("Done creating mag thread");
|
||||
Serial.println("Are you alive?");
|
||||
LedManager = new WS2812Manager(GPIO_NUM_5,1);
|
||||
xTaskCreate(WS2812Manager::RunRGB, "RGB thread", 4096, LedManager, 6, NULL);
|
||||
delay(100);
|
||||
hm = new HellMetServoManager();
|
||||
}
|
||||
|
||||
void loop(){
|
||||
rtosLoop(NULL);
|
||||
}
|
||||
@ -0,0 +1,11 @@
|
||||
|
||||
This directory is intended for PlatformIO Test Runner and project tests.
|
||||
|
||||
Unit Testing is a software testing method by which individual units of
|
||||
source code, sets of one or more MCU program modules together with associated
|
||||
control data, usage procedures, and operating procedures, are tested to
|
||||
determine whether they are fit for use. Unit testing finds problems early
|
||||
in the development cycle.
|
||||
|
||||
More information about PlatformIO Unit Testing:
|
||||
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
|
||||
Loading…
Reference in new issue