yorick.geoffre
/
ARHS-helmet
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

importing existing files

Browse Source
master
Yorick GEOFFRE 3 years ago
commit 582860d2ab
18 changed files with 670 additions and 0 deletions
Show Stats Download Patch File Download Diff File

5
.gitignore vendored
Unescape View File

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

10
.vscode/extensions.json vendored
Unescape View File

@ -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"
]
}

40
include/HellMetServo.hpp
Unescape View File

@ -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);
};

29
include/I2CManagement.hpp
Unescape View File

@ -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();
};

26
include/PinDefinitions.hpp
Unescape View File

@ -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)

39
include/README
Unescape View File

@ -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

15
include/WS2812Manager.hpp
Unescape View File

@ -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);
};

24
include/hmc5883L.hpp
Unescape View File

@ -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();
};

1
include/main.hpp
Unescape View File

@ -0,0 +1 @@
#include "HellMetServo.hpp"

46
lib/README
Unescape View File

@ -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

25
platformio.ini
Unescape View File

@ -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

116
src/HellMetServo.cpp
Unescape View File

@ -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);
}

54
src/I2CManagement.cpp
Unescape View File

@ -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).");
}

14
src/PinDefinitions.cpp
Unescape View File

@ -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);
}

75
src/WS2812Manager.cpp
Unescape View File

@ -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);
}
}

67
src/hmc5883L.cpp
Unescape View File

@ -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);
}

73
src/main.cpp
Unescape View File

@ -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);
}

11
test/README
Unescape View File

@ -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
Write Preview
Loading…
Cancel
Save