updated code and sch
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -24,6 +24,23 @@ X Pin_4 4 -200 -200 150 R 50 50 1 1 P
|
||||
ENDDRAW
|
||||
ENDDEF
|
||||
#
|
||||
# Connector_TestPoint
|
||||
#
|
||||
DEF Connector_TestPoint TP 0 30 N N 1 F N
|
||||
F0 "TP" 0 270 50 H V C CNN
|
||||
F1 "Connector_TestPoint" 0 200 50 H V C CNN
|
||||
F2 "" 200 0 50 H I C CNN
|
||||
F3 "" 200 0 50 H I C CNN
|
||||
$FPLIST
|
||||
Pin*
|
||||
Test*
|
||||
$ENDFPLIST
|
||||
DRAW
|
||||
C 0 130 30 0 1 0 N
|
||||
X 1 1 0 0 100 U 50 50 1 1 P
|
||||
ENDDRAW
|
||||
ENDDEF
|
||||
#
|
||||
# Connector_USB_B_Micro
|
||||
#
|
||||
DEF Connector_USB_B_Micro J 0 40 Y Y 1 F N
|
||||
@@ -133,6 +150,44 @@ X ~ 2 0 -150 50 U 50 50 1 1 P
|
||||
ENDDRAW
|
||||
ENDDEF
|
||||
#
|
||||
# Device_R_Pack04
|
||||
#
|
||||
DEF Device_R_Pack04 RN 0 0 Y N 1 F N
|
||||
F0 "RN" -300 0 50 V V C CNN
|
||||
F1 "Device_R_Pack04" 200 0 50 V V C CNN
|
||||
F2 "" 275 0 50 V I C CNN
|
||||
F3 "" 0 0 50 H I C CNN
|
||||
$FPLIST
|
||||
DIP*
|
||||
SOIC*
|
||||
R*Array*Concave*
|
||||
R*Array*Convex*
|
||||
$ENDFPLIST
|
||||
DRAW
|
||||
S -250 -95 150 95 0 1 10 f
|
||||
S -225 75 -175 -75 0 1 10 N
|
||||
S -125 75 -75 -75 0 1 10 N
|
||||
S -25 75 25 -75 0 1 10 N
|
||||
S 75 75 125 -75 0 1 10 N
|
||||
P 2 0 1 0 -200 -100 -200 -75 N
|
||||
P 2 0 1 0 -200 75 -200 100 N
|
||||
P 2 0 1 0 -100 -100 -100 -75 N
|
||||
P 2 0 1 0 -100 75 -100 100 N
|
||||
P 2 0 1 0 0 -100 0 -75 N
|
||||
P 2 0 1 0 0 75 0 100 N
|
||||
P 2 0 1 0 100 -100 100 -75 N
|
||||
P 2 0 1 0 100 75 100 100 N
|
||||
X R1.1 1 -200 -200 100 U 50 50 1 1 P
|
||||
X R2.1 2 -100 -200 100 U 50 50 1 1 P
|
||||
X R3.1 3 0 -200 100 U 50 50 1 1 P
|
||||
X R4.1 4 100 -200 100 U 50 50 1 1 P
|
||||
X R4.2 5 100 200 100 D 50 50 1 1 P
|
||||
X R3.2 6 0 200 100 D 50 50 1 1 P
|
||||
X R2.2 7 -100 200 100 D 50 50 1 1 P
|
||||
X R1.2 8 -200 200 100 D 50 50 1 1 P
|
||||
ENDDRAW
|
||||
ENDDEF
|
||||
#
|
||||
# Device_R_Potentiometer_Trim
|
||||
#
|
||||
DEF Device_R_Potentiometer_Trim RV 0 40 Y N 1 F N
|
||||
@@ -153,27 +208,23 @@ X 3 3 0 -150 50 U 50 50 1 1 P
|
||||
ENDDRAW
|
||||
ENDDEF
|
||||
#
|
||||
# Diode_BAV70
|
||||
# Diode_1N4448WS
|
||||
#
|
||||
DEF Diode_BAV70 D 0 30 Y N 1 F N
|
||||
F0 "D" 25 -100 50 H V L CNN
|
||||
F1 "Diode_BAV70" 0 100 50 H V C CNN
|
||||
F2 "Package_TO_SOT_SMD:SOT-23" 0 0 50 H I C CNN
|
||||
DEF Diode_1N4448WS D 0 40 N N 1 F N
|
||||
F0 "D" 0 100 50 H V C CNN
|
||||
F1 "Diode_1N4448WS" 0 -100 50 H V C CNN
|
||||
F2 "Diode_SMD:D_SOD-323" 0 -175 50 H I C CNN
|
||||
F3 "" 0 0 50 H I C CNN
|
||||
ALIAS 1N4448WS BAS316
|
||||
$FPLIST
|
||||
SOT?23*
|
||||
D*SOD?323*
|
||||
$ENDFPLIST
|
||||
DRAW
|
||||
C 0 0 10 0 1 0 F
|
||||
P 2 0 1 0 -150 0 150 0 N
|
||||
P 2 0 1 0 0 0 0 -100 N
|
||||
P 3 0 1 10 -50 -50 -50 50 -50 50 N
|
||||
P 3 0 1 10 50 -50 50 50 50 50 N
|
||||
P 6 0 1 10 -150 50 -50 0 -150 -50 -150 50 -150 50 -150 50 N
|
||||
P 6 0 1 10 150 -50 50 0 150 50 150 -50 150 -50 150 -50 N
|
||||
X A 1 -300 0 150 R 50 50 0 1 P
|
||||
X A 2 300 0 150 L 50 50 0 1 P
|
||||
X K 3 0 -200 100 U 50 50 0 1 P
|
||||
P 2 0 1 10 -50 50 -50 -50 N
|
||||
P 2 0 1 0 50 0 -50 0 N
|
||||
P 4 0 1 10 50 50 50 -50 -50 0 50 50 N
|
||||
X K 1 -150 0 100 R 50 50 1 1 P
|
||||
X A 2 150 0 100 L 50 50 1 1 P
|
||||
ENDDRAW
|
||||
ENDDEF
|
||||
#
|
||||
|
||||
+1954
-1694
File diff suppressed because it is too large
Load Diff
+1987
-1733
File diff suppressed because it is too large
Load Diff
+499
-281
File diff suppressed because it is too large
Load Diff
+525
-274
File diff suppressed because it is too large
Load Diff
+3256
-134
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
@@ -4,5 +4,13 @@
|
||||
"path": "."
|
||||
}
|
||||
],
|
||||
"settings": {}
|
||||
"settings": {
|
||||
"files.associations": {
|
||||
"*.tcc": "cpp",
|
||||
"fstream": "cpp",
|
||||
"istream": "cpp",
|
||||
"ostream": "cpp",
|
||||
"sstream": "cpp"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3,7 +3,7 @@
|
||||
#include "Arduino.h"
|
||||
|
||||
#ifndef HARDWAREVERSION
|
||||
#define HARDWAREVERSION 11
|
||||
#define HARDWAREVERSION 12
|
||||
#endif
|
||||
|
||||
|
||||
@@ -30,6 +30,8 @@
|
||||
#define MCU_BUT_MIN PB4
|
||||
#define MCU_BUT_PLUS PB5
|
||||
#elif HARDWAREVERSION == 11
|
||||
#define HASLEDS
|
||||
|
||||
#define MOT_STCK0 5
|
||||
#define MOT_DIR0 4
|
||||
#define MOT_RST0 14
|
||||
@@ -38,7 +40,7 @@
|
||||
#define MOT_DIR1 16
|
||||
#define MOT_RST1 18
|
||||
|
||||
#define MOT_VREF A18 //25
|
||||
#define MOT_VREF 25
|
||||
#define MOT_EN 13
|
||||
|
||||
#define ADDR0 33
|
||||
@@ -46,7 +48,35 @@
|
||||
#define ADDR2 35
|
||||
#define ADDR3 34
|
||||
|
||||
#define I2C_CLK 22
|
||||
#define I2C_SCL 22
|
||||
#define I2C_SDA 21
|
||||
|
||||
#define PROX_LED 27
|
||||
#define LED_DATA 26
|
||||
#define PROX_IN 36
|
||||
|
||||
#define MCU_BUT_MIN 19
|
||||
#define MCU_BUT_PLUS 23
|
||||
|
||||
#elif HARDWAREVERSION == 12
|
||||
#define HASLEDS
|
||||
#define MOT_A1 17
|
||||
#define MOT_A2 16
|
||||
#define MOT_A3 18
|
||||
|
||||
#define MOT_B1 19
|
||||
#define MOT_B2 20
|
||||
#define MOT_B3 21
|
||||
|
||||
#define MOT_VREF 25
|
||||
#define MOT_EN 13
|
||||
|
||||
#define ADDR0 33
|
||||
#define ADDR1 32
|
||||
#define ADDR2 35
|
||||
#define ADDR3 34
|
||||
|
||||
#define I2C_SCL 22
|
||||
#define I2C_SDA 21
|
||||
|
||||
#define PROX_LED 27
|
||||
|
||||
+15
-23
@@ -11,29 +11,6 @@
|
||||
[env]
|
||||
framework = arduino
|
||||
lib_ldf_mode = deep+
|
||||
lib_deps =
|
||||
http://192.168.2.3/Bonobo.Git.Server/Accelstepper.git
|
||||
Fastled/FastLED @ ^3.4.0
|
||||
makuna/NeoPixelBus @ ^2.6.2
|
||||
|
||||
|
||||
; [env:clockclock_hwV10_L422]
|
||||
;platform = ststm32
|
||||
; board = STM32L422KB
|
||||
; build_flags =
|
||||
; -DHARDWAREVERSION=10
|
||||
; -O2
|
||||
|
||||
|
||||
; STM32L031K6
|
||||
; [env:clockclock_hwV10_L301]
|
||||
; platform = ststm32
|
||||
; board = STM32L031K6
|
||||
; upload_port = stlink
|
||||
; debug_tool = stlink
|
||||
; build_flags =
|
||||
; -DHARDWAREVERSION=10
|
||||
; -O2
|
||||
|
||||
[env:clockclock_hwV11_ESP32]
|
||||
platform = espressif32
|
||||
@@ -41,5 +18,20 @@ board = esp32dev
|
||||
monitor_speed = 115200
|
||||
build_flags =
|
||||
-DHARDWAREVERSION=11
|
||||
lib_deps =
|
||||
ayashki/ESP32 I2C Slave@^0.1.0
|
||||
Fastled/FastLED @ ^3.4.0
|
||||
http://192.168.2.3/Bonobo.Git.Server/Accelstepper.git
|
||||
|
||||
|
||||
|
||||
[env:clockclock_hwV12_ESP32]
|
||||
platform = espressif32
|
||||
board = esp32dev
|
||||
monitor_speed = 115200
|
||||
build_flags =
|
||||
-DHARDWAREVERSION=12
|
||||
lib_deps =
|
||||
ayashki/ESP32 I2C Slave@^0.1.0
|
||||
Fastled/FastLED @ ^3.4.0
|
||||
http://192.168.2.3/Bonobo.Git.Server/Accelstepper.git
|
||||
@@ -9,4 +9,8 @@
|
||||
|
||||
void initProximity( void );
|
||||
void handleProximity( void );
|
||||
void getProximity( void );
|
||||
uint16_t getProximity(void);
|
||||
bool getProxInitState( void);
|
||||
void enableProximity(void);
|
||||
void disableProximity(void);
|
||||
|
||||
|
||||
@@ -3,4 +3,5 @@
|
||||
void initBoard( void )
|
||||
{
|
||||
Serial.begin(115200);
|
||||
delay(500);
|
||||
}
|
||||
@@ -6,6 +6,7 @@ Button buttonMin(MCU_BUT_MIN, 25UL, true, true);
|
||||
|
||||
void handleButtons(void)
|
||||
{
|
||||
Serial.print("|Bu");
|
||||
if (buttonPlus.read())
|
||||
{
|
||||
increaseMotor(MOTOR0);
|
||||
|
||||
+140
-20
@@ -1,8 +1,7 @@
|
||||
#include "comms.h"
|
||||
#include "board.h"
|
||||
#include "Wire.h"
|
||||
|
||||
|
||||
uint64_t commsTimerSlave = 0;
|
||||
uint64_t commsTimerMaster = 0;
|
||||
|
||||
uint8_t getAddress(void)
|
||||
{
|
||||
@@ -16,18 +15,9 @@ uint8_t getAddress(void)
|
||||
return address;
|
||||
}
|
||||
|
||||
void receiveI2CEvent(int bytes) {
|
||||
Wire.read(); // read one character from the I2C
|
||||
}
|
||||
|
||||
void requestI2CEvent() {
|
||||
Wire.read(); // read one character from the I2C
|
||||
}
|
||||
|
||||
|
||||
uint8_t address;
|
||||
|
||||
void initComms( void)
|
||||
void initComms(void)
|
||||
{
|
||||
Serial.print("init Comms");
|
||||
pinMode(ADDR0, INPUT_PULLUP);
|
||||
@@ -37,18 +27,148 @@ void initComms( void)
|
||||
delay(10);
|
||||
address = getAddress();
|
||||
Serial.printf("selected address: %u", address);
|
||||
// Start the I2C Bus as Slave on address 9
|
||||
Wire.begin(address);
|
||||
// Attach a function to trigger when something is received.
|
||||
//Wire.onReceive(receiveI2CEvent);
|
||||
//Wire.onRequest(requestI2CEvent);
|
||||
if (address == COMMSBASE)
|
||||
{
|
||||
//init as master
|
||||
initCommsMaster();
|
||||
}
|
||||
else
|
||||
{
|
||||
//init as slave
|
||||
initCommsSlave(address);
|
||||
}
|
||||
Serial.println(" :Ok");
|
||||
}
|
||||
|
||||
void handleComms ( void )
|
||||
void handleComms(void)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
// Simple I2C protocol v0.2 for Arduino
|
||||
// Master side program
|
||||
// (c) 2014 Ignas Gramba
|
||||
//
|
||||
|
||||
uint16_t a, b, c, d, e; // arguments
|
||||
c_message message;
|
||||
|
||||
void initCommsMaster(void)
|
||||
{
|
||||
Wire.begin(); // join i2c bus (address optional for master)
|
||||
message.setCommand(2); // initialise test values
|
||||
a = 105;
|
||||
b = 2350;
|
||||
c = 4587;
|
||||
d = 12587;
|
||||
e = 12;
|
||||
}
|
||||
|
||||
void c_message::makeDataPacket(uint16_t aa, uint16_t bb, uint16_t cc, uint16_t dd, uint16_t ee)
|
||||
{
|
||||
_dataPacket[0] = _command;
|
||||
_dataPacket[1] = aa >> 8;
|
||||
_dataPacket[2] = aa & 255;
|
||||
_dataPacket[3] = bb >> 8;
|
||||
_dataPacket[4] = bb & 255;
|
||||
_dataPacket[5] = cc >> 8;
|
||||
_dataPacket[6] = cc & 255;
|
||||
_dataPacket[7] = dd >> 8;
|
||||
_dataPacket[8] = dd & 255;
|
||||
_dataPacket[9] = ee >> 8;
|
||||
_dataPacket[10] = ee & 255;
|
||||
}
|
||||
|
||||
void receiveDataPacket(int howMany)
|
||||
{
|
||||
// if (howMany != 11) return; // Error
|
||||
message.setLastCommand(Wire.read());
|
||||
a = Wire.read() << 8 | Wire.read();
|
||||
b = Wire.read() << 8 | Wire.read();
|
||||
c = Wire.read() << 8 | Wire.read();
|
||||
d = Wire.read() << 8 | Wire.read();
|
||||
e = Wire.read() << 8 | Wire.read();
|
||||
}
|
||||
|
||||
void c_message::sendDataPacket()
|
||||
{
|
||||
Wire.beginTransmission(message.getSlaveID());
|
||||
Wire.write(_dataPacket, 11);
|
||||
delay(10);
|
||||
}
|
||||
|
||||
int receiveResponse()
|
||||
{
|
||||
int receivedValue = 0xffff;
|
||||
int available = Wire.requestFrom(message.getSlaveID(), 2);
|
||||
if (available == 2)
|
||||
{
|
||||
receivedValue = Wire.read() << 8 | Wire.read(); // combine two bytes into integer
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.print("ERROR: Unexpected number of bytes received - ");
|
||||
Serial.println(available);
|
||||
}
|
||||
Wire.endTransmission(true);
|
||||
return receivedValue;
|
||||
}
|
||||
|
||||
void slavesRespond()
|
||||
{
|
||||
|
||||
int returnValue = 0;
|
||||
|
||||
switch (message.getLastCommnand())
|
||||
{
|
||||
case 0: // No new command was received
|
||||
returnValue = 1; // i.e. error code #1
|
||||
break;
|
||||
|
||||
case 1: // Some function
|
||||
|
||||
break;
|
||||
|
||||
case 2: // Our test function
|
||||
returnValue = message.sumFunction(a, b, c, d, e);
|
||||
break;
|
||||
}
|
||||
|
||||
byte buffer[2]; // split int value into two bytes buffer
|
||||
buffer[0] = returnValue >> 8;
|
||||
buffer[1] = returnValue & 255;
|
||||
Wire.write(buffer, 2); // return response to last command
|
||||
message.setLastCommand(0); // null last Master's command
|
||||
}
|
||||
|
||||
int c_message::sumFunction(int aa, int bb, int cc, int dd, int ee)
|
||||
{
|
||||
// of course for summing 5 integers You need long type of return,
|
||||
// but this is only illustration. Test values doesn't overflow
|
||||
|
||||
int result = aa + bb + cc + dd + ee;
|
||||
return result;
|
||||
}
|
||||
|
||||
void handleCommsMaster()
|
||||
{
|
||||
message.makeDataPacket(a, b, c, d, e);
|
||||
|
||||
message.sendDataPacket();
|
||||
|
||||
int response = receiveResponse();
|
||||
Serial.print("Slave response: ");
|
||||
Serial.println(response);
|
||||
}
|
||||
|
||||
void initCommsSlave(int SlaveDeviceId)
|
||||
{
|
||||
message.setSlaveID(SlaveDeviceId);
|
||||
bool success = WireSlave.begin(I2C_SDA, I2C_SCL, SlaveDeviceId);
|
||||
if (!success)
|
||||
{
|
||||
Serial.print("I2C slave init failed");
|
||||
}
|
||||
// Wire.begin(SlaveDeviceId); // join i2c bus with Slave ID
|
||||
WireSlave.onReceive(receiveDataPacket); // register talk event
|
||||
WireSlave.onRequest(slavesRespond); // register callback event
|
||||
}
|
||||
|
||||
+30
-17
@@ -1,29 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Wire.h"
|
||||
#include <WireSlave.h>
|
||||
#include "board.h"
|
||||
|
||||
#define MSGLEN 10
|
||||
#define MSGLEN 128
|
||||
#define PACKETSIZE 11
|
||||
#define COMMSBASE 80
|
||||
|
||||
class c_message
|
||||
{
|
||||
uint8_t _buffer[MSGLEN];
|
||||
uint8_t _bufIndex = 0;
|
||||
bool validheader;
|
||||
byte _dataPacket[11] = {
|
||||
0,
|
||||
};
|
||||
byte _lastMasterCommand = 0;
|
||||
byte _command;
|
||||
int _slaveID;
|
||||
|
||||
public:
|
||||
c_message();
|
||||
|
||||
void addByte(uint8_t byte)
|
||||
{
|
||||
_buffer[_bufIndex++] = byte;
|
||||
if(_bufIndex >= MSGLEN)
|
||||
{
|
||||
_bufIndex = 0;
|
||||
}
|
||||
|
||||
}
|
||||
c_message(void) {}
|
||||
void makeDataPacket(uint16_t aa, uint16_t bb, uint16_t cc, uint16_t dd, uint16_t ee);
|
||||
void sendDataPacket();
|
||||
int sumFunction(int aa, int bb, int cc, int dd, int ee);
|
||||
|
||||
void setCommand(byte command) { _command = command; }
|
||||
byte getCommand(void) { return _command; }
|
||||
byte getLastCommnand(void) { return _lastMasterCommand; }
|
||||
void setLastCommand(byte command) { _lastMasterCommand = command; }
|
||||
void setSlaveID(int id) { _slaveID = id;}
|
||||
int getSlaveID( void ){ return _slaveID;}
|
||||
};
|
||||
|
||||
void initComms( void);
|
||||
void handleComms ( void );
|
||||
void receiveDataPacket(int howMany);
|
||||
void slavesRespond();
|
||||
|
||||
void initComms(void);
|
||||
void handleComms(void);
|
||||
|
||||
void initCommsMaster(void);
|
||||
void initCommsSlave(int SlaveDeviceId);
|
||||
|
||||
+47
-18
@@ -1,20 +1,14 @@
|
||||
#include "led.h"
|
||||
|
||||
//CRGB leds[NUM_LEDS];
|
||||
#ifdef HASLEDS
|
||||
CRGB leds[NUM_LEDS];
|
||||
|
||||
uint64_t ledtimer1 = 0;
|
||||
uint8_t hue = 0;
|
||||
bool ledstate = 0;
|
||||
|
||||
uint64_t ledtimer1 = 0;
|
||||
uint8_t hue = 0;
|
||||
bool ledstate = 0;
|
||||
#define colorSaturation 128
|
||||
|
||||
NeoPixelBus<DotStarBgrFeature, DotStarMethod> LEDS(NUM_LEDS, LED_DATA);
|
||||
|
||||
RgbColor red(colorSaturation, 0, 0);
|
||||
RgbColor green(0, colorSaturation, 0);
|
||||
RgbColor blue(0, 0, colorSaturation);
|
||||
RgbColor white(colorSaturation);
|
||||
RgbColor black(0);
|
||||
void fadeall() { for(int i = 0; i < NUM_LEDS; i++) { leds[i].nscale8(250); } }
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -25,20 +19,55 @@ RgbColor black(0);
|
||||
void initLeds(void)
|
||||
{
|
||||
Serial.print("Init Leds: ");
|
||||
LEDS.begin();
|
||||
LEDS.ClearTo(black);
|
||||
#ifdef HASLEDS
|
||||
LEDS.addLeds<SK6812,LED_DATA,RGB>(leds,NUM_LEDS);
|
||||
LEDS.setBrightness(50);
|
||||
|
||||
for(int i = 0; i < NUM_LEDS; i++) {
|
||||
// Set the i'th led to red
|
||||
leds[i] = CRGB::White;
|
||||
// Show the leds
|
||||
|
||||
}
|
||||
FastLED.show();
|
||||
#endif
|
||||
Serial.println("OK");
|
||||
}
|
||||
|
||||
void handleLeds(void)
|
||||
{
|
||||
#ifdef HASLEDS
|
||||
uint64_t currentmillis = millis();
|
||||
if (currentmillis - ledtimer1 > LEDINTERVAL)
|
||||
{
|
||||
|
||||
static uint8_t hue = 0;
|
||||
Serial.print("|L");
|
||||
// First slide the led in one direction
|
||||
for(int i = 0; i < NUM_LEDS; i++) {
|
||||
// Set the i'th led to red
|
||||
leds[i] = CHSV(hue++, 255, 255);
|
||||
// Show the leds
|
||||
FastLED.show();
|
||||
// now that we've shown the leds, reset the i'th led to black
|
||||
// leds[i] = CRGB::Black;
|
||||
fadeall();
|
||||
// Wait a little bit before we loop around and do it again
|
||||
//delay(10);
|
||||
}
|
||||
// Now go in the other direction.
|
||||
for(int i = (NUM_LEDS)-1; i >= 0; i--) {
|
||||
// Set the i'th led to red
|
||||
leds[i] = CHSV(hue++, 255, 255);
|
||||
// Show the leds
|
||||
FastLED.show();
|
||||
// now that we've shown the leds, reset the i'th led to black
|
||||
// leds[i] = CRGB::Black;
|
||||
fadeall();
|
||||
// Wait a little bit before we loop around and do it again
|
||||
//delay(10);
|
||||
}
|
||||
|
||||
ledtimer1 = currentmillis;
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@@ -1,8 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include "board.h"
|
||||
#include <FastLED.h>
|
||||
#include <NeoPixelBus.h>
|
||||
#ifdef HASLEDS
|
||||
#define FASTLED_INTERNAL
|
||||
#include <FastLED.h>
|
||||
#endif
|
||||
|
||||
#define NUM_LEDS 4
|
||||
#define LEDINTERVAL 10
|
||||
|
||||
+37
-7
@@ -15,16 +15,46 @@ void setup() {
|
||||
initMovement();
|
||||
initProximity();
|
||||
initLeds();
|
||||
initComms();
|
||||
|
||||
//initComms();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
void handle1msTasks(void)
|
||||
{
|
||||
handleMotors();
|
||||
handleButtons();
|
||||
handleMovement();
|
||||
handleProximity();
|
||||
handleLeds();
|
||||
handleComms();
|
||||
handleMovement();
|
||||
}
|
||||
|
||||
void handle10msTasks(void)
|
||||
{
|
||||
|
||||
handleLeds();
|
||||
}
|
||||
|
||||
uint64_t looptime = 0;
|
||||
uint64_t lastMicros = 0;
|
||||
uint16_t task10msCnt = 0;
|
||||
|
||||
void loop() {
|
||||
|
||||
uint64_t currentMicros = micros();
|
||||
//Serial.print("begin");
|
||||
if(lastMicros - currentMicros > 1000)
|
||||
{
|
||||
Serial.print("1msTasks:");
|
||||
handle1msTasks();
|
||||
currentMicros = lastMicros;
|
||||
task10msCnt++;
|
||||
Serial.printf("|Done(%.2fms)\n",((double)(micros() - currentMicros))/1000);
|
||||
}
|
||||
|
||||
if(task10msCnt >= 10)
|
||||
{
|
||||
Serial.print("10msTasks:");
|
||||
handle10msTasks();
|
||||
task10msCnt = 0;
|
||||
Serial.println("|Done");
|
||||
}
|
||||
|
||||
}
|
||||
+305
-47
@@ -1,53 +1,79 @@
|
||||
#include "motor.h"
|
||||
#include "arduino.h"
|
||||
#include "board.h"
|
||||
|
||||
#include "MultiStepper.h"
|
||||
//#include "MultiStepper.h"
|
||||
#if HARDWAREVERSION == 12
|
||||
c_motor motor0(MOTOR0, MOT_A1, MOT_A2, MOT_A3, "M0");
|
||||
c_motor motor1(MOTOR1, MOT_B1, MOT_B2, MOT_B3, "M1");
|
||||
#else
|
||||
c_motor motor0(MOTOR0, MOT_STCK0, MOT_DIR0, MOT_EN, MOT_RST0, "M0");
|
||||
c_motor motor1(MOTOR1, MOT_STCK1, MOT_DIR1, MOT_EN, MOT_RST1, "M1");
|
||||
#endif
|
||||
|
||||
c_motor motor0(MOT_STCK0, MOT_DIR0, MOT_EN, MOT_RST0);
|
||||
c_motor motor1(MOT_STCK1, MOT_DIR1, MOT_EN, MOT_RST1);
|
||||
std::vector<c_motor *> motor_list;
|
||||
|
||||
MultiStepper motors;
|
||||
bool setvref = false;
|
||||
|
||||
void c_motor::init(bool setvref)
|
||||
void c_motor::init()
|
||||
{
|
||||
Serial.print("init Motors: ");
|
||||
pinMode(_rst, OUTPUT);
|
||||
pinMode(_en, OUTPUT);
|
||||
pinMode(_sclk, OUTPUT);
|
||||
pinMode(_dir, OUTPUT);
|
||||
Serial.printf("init %s: ", _motorName);
|
||||
#if HARDWAREVERSION == 12
|
||||
pinMode(_pin1, OUTPUT);
|
||||
pinMode(_pin2, OUTPUT);
|
||||
pinMode(_pin3, OUTPUT);
|
||||
#else
|
||||
pinMode(_pin3, OUTPUT);
|
||||
pinMode(_pin4, OUTPUT);
|
||||
#endif
|
||||
|
||||
digitalWrite(_rst, 0);
|
||||
digitalWrite(_sclk, 0);
|
||||
digitalWrite(_dir, 0);
|
||||
digitalWrite(_en, 0);
|
||||
digitalWrite(_pin3, 0); //rst
|
||||
digitalWrite(_pin4, 0); //en
|
||||
|
||||
delay(200);
|
||||
|
||||
digitalWrite(_rst, 1);
|
||||
digitalWrite(_pin3, 1); //rst
|
||||
digitalWrite(_pin4, 1); //en
|
||||
|
||||
if (setvref)
|
||||
#if HARDWAREVERSION != 12
|
||||
if (!setvref)
|
||||
{
|
||||
Serial.print("Setup Vref: ");
|
||||
ledcSetup(0,12800,8);
|
||||
//pinMode(MOT_VREF, OUTPUT);
|
||||
ledcAttachPin(MOT_VREF,0);
|
||||
ledcWrite(MOT_VREF, 32);
|
||||
dacWrite(MOT_VREF, 196);
|
||||
setvref = true;
|
||||
}
|
||||
Serial.println("OK");
|
||||
#endif
|
||||
_initDone = true;
|
||||
Serial.print("OK");
|
||||
}
|
||||
|
||||
void c_motor::begin(void)
|
||||
{
|
||||
_stepper.begin(RPM, MICROSTEPS);
|
||||
_stepper.enable();
|
||||
_stepper.setMaxSpeed(MOTORMAXSPEED);
|
||||
_stepper.setAcceleration(400.0);
|
||||
}
|
||||
|
||||
void c_motor::run(void)
|
||||
{
|
||||
if (!_stepper.distanceToGo())
|
||||
{
|
||||
Serial.printf("|%s-R(%lu)", _motorName, _stepper.distanceToGo());
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.printf("|%s-D", _motorName);
|
||||
}
|
||||
_stepper.run();
|
||||
}
|
||||
|
||||
void c_motor::move(long pos)
|
||||
{
|
||||
_stepper.move(pos);
|
||||
}
|
||||
|
||||
void c_motor::decreasePos(void)
|
||||
{
|
||||
if (ready())
|
||||
{
|
||||
_stepper.move(-OFFSET);
|
||||
move(-OFFSET);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -55,28 +81,207 @@ void c_motor::increasePos(void)
|
||||
{
|
||||
if (ready())
|
||||
{
|
||||
_stepper.move(OFFSET);
|
||||
move(OFFSET);
|
||||
}
|
||||
}
|
||||
|
||||
bool c_motor::getIndex(e_motor index)
|
||||
{
|
||||
return (index == _index);
|
||||
}
|
||||
|
||||
bool c_motor::ready(void)
|
||||
{
|
||||
return (_stepper.getCurrentState() == _stepper.State::STOPPED);
|
||||
if (_stepper.distanceToGo() != 0)
|
||||
{
|
||||
Serial.printf("|%s-BUSY", _motorName);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void c_motor::moveTo(long pos)
|
||||
{
|
||||
_stepper.moveTo(pos);
|
||||
}
|
||||
|
||||
bool c_motor::startCalibration(void)
|
||||
{
|
||||
if (_calibrationState == wait)
|
||||
{
|
||||
_calibrationState = detectSensor;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool c_motor::calibrate()
|
||||
{
|
||||
if (!ready() && (_calibrationState != calibrated))
|
||||
{
|
||||
//wait for the stepper to be ready
|
||||
Serial.print("|CalBusy");
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (_calibrationState)
|
||||
{
|
||||
case initCal:
|
||||
{
|
||||
//wait for modules to be ready
|
||||
Serial.print("|Calinit");
|
||||
if (getProxInitState() && _initDone)
|
||||
{
|
||||
_calibrationState = clearSensor;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case clearSensor:
|
||||
{
|
||||
//rotate until sensor is cleared
|
||||
Serial.print("|calClear");
|
||||
if (getProximity() < CAL_IDLE_THR)
|
||||
{
|
||||
move(CLEARSTEPS);
|
||||
}
|
||||
else
|
||||
{
|
||||
_stepper.stop();
|
||||
_calibrationState = wait;
|
||||
_stepper.setCurrentPosition(0);
|
||||
_storePos = _stepper.currentPosition();
|
||||
}
|
||||
}
|
||||
break;
|
||||
case wait:
|
||||
{
|
||||
//wait for StartCalibration
|
||||
Serial.print("|calWait");
|
||||
}
|
||||
break;
|
||||
case detectSensor:
|
||||
{
|
||||
//rotate and detect sensor
|
||||
Serial.print("|calDet");
|
||||
if (getProximity() > SENSORTHRESHOLD)
|
||||
{
|
||||
move(CALIBRATIONSTEPS);
|
||||
if (_stepper.currentPosition() - _storePos > MOTOR_STEPS)
|
||||
{
|
||||
_calibrationState = calibrationError;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_stepper.stop();
|
||||
_calSensorStart = _stepper.currentPosition();
|
||||
Serial.printf("-start=%u", _calSensorStart);
|
||||
_calibrationState = detectwidth;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case detectwidth:
|
||||
{
|
||||
//rotate and detect !sensor
|
||||
Serial.print("|calWidth");
|
||||
if (getProximity() < SENSORTHRESHOLD)
|
||||
{
|
||||
move(CALIBRATIONSTEPS);
|
||||
}
|
||||
else
|
||||
{
|
||||
_stepper.stop();
|
||||
_calSensorEnd = _stepper.currentPosition();
|
||||
Serial.printf("-end=%u", _calSensorEnd);
|
||||
_calibrationState = calculate;
|
||||
if (_stepper.currentPosition() - _storePos > MOTOR_STEPS)
|
||||
{
|
||||
_calibrationState = calibrationError;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case calculate:
|
||||
{
|
||||
//calculate center
|
||||
Serial.print("|calCalc");
|
||||
uint16_t pointerWidth = _calSensorEnd - _calSensorStart;
|
||||
uint16_t pointerCenter = pointerWidth / 2;
|
||||
_stepper.setCurrentPosition(pointerCenter);
|
||||
_calibrationState = calibrated;
|
||||
_calibrated = true;
|
||||
Serial.printf("-center=%u,", pointerCenter);
|
||||
moveTo(CLEARSTEPS);
|
||||
}
|
||||
break;
|
||||
case calibrated:
|
||||
{
|
||||
return true;
|
||||
//calibration done
|
||||
}
|
||||
break;
|
||||
default:
|
||||
case calibrationError:
|
||||
{
|
||||
Serial.print("|CALERROR");
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
//calibration not finished, return false
|
||||
return false;
|
||||
}
|
||||
|
||||
bool c_motor::isCalibrated(void)
|
||||
{
|
||||
if (_calibrated)
|
||||
{
|
||||
Serial.printf("%s calibrated", _motorName);
|
||||
return true;
|
||||
}
|
||||
Serial.printf("%s not calibrated", _motorName);
|
||||
return false;
|
||||
}
|
||||
|
||||
/*-----------------------------------*/
|
||||
/* generic functions */
|
||||
/*-----------------------------------*/
|
||||
|
||||
c_motor *getMotor(e_motor index)
|
||||
{
|
||||
for (auto &&thismotor : motor_list)
|
||||
{
|
||||
if (thismotor->getIndex(index))
|
||||
{
|
||||
return thismotor;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void handleMotors(void)
|
||||
{
|
||||
motor0.run();
|
||||
motor1.run();
|
||||
Serial.print("|Mot");
|
||||
for (auto &&thismotor : motor_list)
|
||||
{
|
||||
thismotor->run();
|
||||
}
|
||||
}
|
||||
|
||||
void initMotors(void)
|
||||
{
|
||||
motor0.init(true);
|
||||
motor0.begin();
|
||||
|
||||
motor1.init();
|
||||
motor1.begin();
|
||||
Serial.print("init motors:");
|
||||
motor_list.clear();
|
||||
motor_list.push_back(&motor0);
|
||||
motor_list.push_back(&motor1);
|
||||
Serial.printf("init vector(%u):", motor_list.size());
|
||||
for (auto &&thismotor : motor_list)
|
||||
{
|
||||
thismotor->init();
|
||||
thismotor->begin();
|
||||
}
|
||||
Serial.println(" :OK");
|
||||
}
|
||||
|
||||
void SetMotors(long pos0, long pos1)
|
||||
@@ -85,26 +290,79 @@ void SetMotors(long pos0, long pos1)
|
||||
motor1.moveTo(pos1);
|
||||
}
|
||||
|
||||
void moveMotorTo(e_motor motor, long pos)
|
||||
{
|
||||
c_motor *thismotor = getMotor(motor);
|
||||
if (thismotor == NULL)
|
||||
{
|
||||
Serial.print("|MoveNULLPTR");
|
||||
return;
|
||||
}
|
||||
thismotor->moveTo(pos);
|
||||
}
|
||||
|
||||
void increaseMotor(e_motor motor)
|
||||
{
|
||||
if (motor == MOTOR0)
|
||||
c_motor *thismotor = getMotor(motor);
|
||||
if (thismotor == NULL)
|
||||
{
|
||||
motor0.increasePos();
|
||||
}
|
||||
if (motor == MOTOR1)
|
||||
{
|
||||
motor1.increasePos();
|
||||
Serial.print("|IncrNULLPTR");
|
||||
return;
|
||||
}
|
||||
thismotor->increasePos();
|
||||
}
|
||||
|
||||
void decreaseMotor(e_motor motor)
|
||||
{
|
||||
if (motor == MOTOR0)
|
||||
c_motor *thismotor = getMotor(motor);
|
||||
if (thismotor == NULL)
|
||||
{
|
||||
motor0.decreasePos();
|
||||
}
|
||||
if (motor == MOTOR1)
|
||||
{
|
||||
motor1.decreasePos();
|
||||
Serial.print("|DecrNULLPTR");
|
||||
return;
|
||||
}
|
||||
thismotor->decreasePos();
|
||||
}
|
||||
|
||||
bool motorsCalibrated(void)
|
||||
{
|
||||
for (auto &&thismotor : motor_list)
|
||||
{
|
||||
if (!thismotor->isCalibrated())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool calibrateMotor(e_motor motor)
|
||||
{
|
||||
c_motor *thismotor = getMotor(motor);
|
||||
if (thismotor == NULL)
|
||||
{
|
||||
Serial.print("|calNULLPTR");
|
||||
return false;
|
||||
}
|
||||
return thismotor->calibrate();
|
||||
}
|
||||
|
||||
e_calStates getCalibrationState(e_motor motor)
|
||||
{
|
||||
c_motor *thismotor = getMotor(motor);
|
||||
if (thismotor == NULL)
|
||||
{
|
||||
Serial.print("|getCalNULLPTR");
|
||||
return calibrationError;
|
||||
}
|
||||
return thismotor->getCalibrationState();
|
||||
}
|
||||
|
||||
void startMotorCalibration(e_motor motor)
|
||||
{
|
||||
c_motor *thismotor = getMotor(motor);
|
||||
if (thismotor == NULL)
|
||||
{
|
||||
Serial.print("|startCalNULLPTR");
|
||||
}
|
||||
thismotor->startCalibration();
|
||||
}
|
||||
+80
-40
@@ -1,65 +1,100 @@
|
||||
#pragma once
|
||||
|
||||
//#include "AccelStepper.h"
|
||||
#include "BasicStepperDriver.h"
|
||||
#include "AccelStepper.h"
|
||||
#include "arduino.h"
|
||||
#include "board.h"
|
||||
#include <vector>
|
||||
#include "Proximity.h"
|
||||
|
||||
#define OFFSET 5
|
||||
|
||||
#define MOTOR_STEPS 360
|
||||
#define RPM 120
|
||||
#define MICROSTEPS 1
|
||||
|
||||
#define MOTOR_STEPS 360 * MICROSTEPS
|
||||
#define RPM 120
|
||||
#define SENSORTHRESHOLD 1000
|
||||
#define CALIBRATIONSTEPS 10 * MICROSTEPS
|
||||
#define CLEARSTEPS 90 * MICROSTEPS
|
||||
#define CAL_IDLE_THR 2000
|
||||
#define MOTORMAXSPEED 90
|
||||
#define OFFSET 5 * MICROSTEPS
|
||||
typedef enum
|
||||
{
|
||||
MOTOR0,
|
||||
MOTOR1
|
||||
} e_motor;
|
||||
|
||||
enum e_calStates
|
||||
{
|
||||
initCal,
|
||||
wait,
|
||||
clearSensor,
|
||||
detectSensor,
|
||||
detectwidth,
|
||||
calculate,
|
||||
calibrated,
|
||||
calibrationError
|
||||
};
|
||||
|
||||
class c_motor
|
||||
{
|
||||
const int _sclk;
|
||||
const int _dir;
|
||||
const int _en;
|
||||
const int _rst;
|
||||
|
||||
long _currentpos;
|
||||
long _targetpos;
|
||||
const e_motor _index;
|
||||
const int _pin1; //sclk;
|
||||
const int _pin2; //_dir;
|
||||
const int _pin3; //_en;
|
||||
const int _pin4; //_rst;
|
||||
|
||||
//AccelStepper _stepper;
|
||||
BasicStepperDriver _stepper;
|
||||
const char *const _motorName;
|
||||
|
||||
bool _calibrated = false;
|
||||
e_calStates _calibrationState = initCal;
|
||||
bool _initDone = false;
|
||||
|
||||
uint16_t _calSensorStart = 0;
|
||||
uint16_t _calSensorEnd = 0;
|
||||
uint16_t _storePos = 0;
|
||||
|
||||
long _currentpos() { return _stepper.currentPosition(); }
|
||||
long _targetpos() { return _stepper.targetPosition(); }
|
||||
|
||||
AccelStepper _stepper;
|
||||
|
||||
public:
|
||||
c_motor(int sclk, int dir, int en, int rst) : _sclk(sclk), _dir(dir), _en(en), _rst(rst),
|
||||
_stepper(MOTOR_STEPS, _sclk, _dir, _en)
|
||||
c_motor(e_motor index, int sclk, int dir, int en, int rst, const char *name)
|
||||
: _index(index),
|
||||
_pin1(sclk),
|
||||
_pin2(dir),
|
||||
_pin3(en),
|
||||
_pin4(rst),
|
||||
_motorName(name),
|
||||
_stepper(AccelStepper::DRIVER, _pin1, _pin2)
|
||||
{
|
||||
}
|
||||
|
||||
void init(bool setvref = false);
|
||||
c_motor(e_motor index, uint8_t A1, uint8_t A2, uint8_t A3, const char *name)
|
||||
: _index(index),
|
||||
_pin1(A1),
|
||||
_pin2(A2),
|
||||
_pin3(A3),
|
||||
_pin4(0),
|
||||
_motorName(name),
|
||||
_stepper(AccelStepper::HALF3WIRE, _pin1, _pin2, _pin3)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void init();
|
||||
void begin(void);
|
||||
|
||||
void moveTo(long pos)
|
||||
{
|
||||
_stepper.startMove(pos);
|
||||
if((_targetpos += pos) > 360)
|
||||
{
|
||||
_targetpos -= 360;
|
||||
}
|
||||
}
|
||||
|
||||
bool ready( void );
|
||||
void run(void)
|
||||
{
|
||||
_stepper.nextAction();
|
||||
|
||||
}
|
||||
|
||||
void moveTo(long pos);
|
||||
void move(long pos);
|
||||
bool ready(void);
|
||||
void run(void);
|
||||
void increasePos(void);
|
||||
void decreasePos(void);
|
||||
|
||||
BasicStepperDriver *getStepper(void)
|
||||
{
|
||||
return &_stepper;
|
||||
}
|
||||
AccelStepper *getStepper(void) { return &_stepper; }
|
||||
bool getIndex(e_motor index);
|
||||
bool isCalibrated(void);
|
||||
bool calibrate();
|
||||
bool startCalibration(void);
|
||||
e_calStates getCalibrationState(void) { return _calibrationState; }
|
||||
};
|
||||
|
||||
void handleMotors(void);
|
||||
@@ -67,3 +102,8 @@ void initMotors(void);
|
||||
void SetMotors(long pos0, long pos1);
|
||||
void increaseMotor(e_motor motor);
|
||||
void decreaseMotor(e_motor motor);
|
||||
bool motorsCalibrated(void);
|
||||
bool calibrateMotor(e_motor motor);
|
||||
e_calStates getCalibrationState(e_motor motor);
|
||||
void startMotorCalibration(e_motor motor);
|
||||
c_motor *getMotor(e_motor index);
|
||||
|
||||
+109
-12
@@ -1,27 +1,124 @@
|
||||
#include "movement.h"
|
||||
#include "motor.h"
|
||||
#include "Proximity.h"
|
||||
|
||||
uint64_t movementTimer1 = 0;
|
||||
#define MOVEMENTINTERVAL 100
|
||||
#define MOVEMENTINTERVAL 10
|
||||
|
||||
void doMovement( void )
|
||||
void initMovement(void)
|
||||
{
|
||||
increaseMotor(MOTOR0);
|
||||
decreaseMotor(MOTOR1);
|
||||
Serial.print("Init Movement:");
|
||||
Serial.print("check calibration:");
|
||||
/* noting to init */
|
||||
motorsCalibrated();
|
||||
Serial.println(" OK");
|
||||
}
|
||||
|
||||
void handleMovement( void )
|
||||
enum calibrationstates
|
||||
{
|
||||
uint64_t currentmillis = millis();
|
||||
if(currentmillis - movementTimer1 > MOVEMENTINTERVAL)
|
||||
cal_idle,
|
||||
cal_init,
|
||||
cal_motor0,
|
||||
cal_motor1,
|
||||
cal_done
|
||||
};
|
||||
|
||||
calibrationstates movementState = cal_idle;
|
||||
|
||||
bool calibrateMovement(void)
|
||||
{
|
||||
switch (movementState)
|
||||
{
|
||||
doMovement();
|
||||
movementTimer1 = currentmillis;
|
||||
case cal_idle:
|
||||
{
|
||||
if (!motorsCalibrated())
|
||||
{
|
||||
movementState = cal_init;
|
||||
Serial.print("|MoCalIdle");
|
||||
}
|
||||
}
|
||||
break;
|
||||
case cal_init:
|
||||
{
|
||||
bool motorReady = true;
|
||||
enableProximity();
|
||||
if (getCalibrationState(MOTOR0) != wait )
|
||||
{
|
||||
calibrateMotor(MOTOR0);
|
||||
motorReady = false;
|
||||
Serial.print("|M0CalInit");
|
||||
|
||||
}
|
||||
if(getCalibrationState(MOTOR1) != wait)
|
||||
{
|
||||
calibrateMotor(MOTOR1);
|
||||
motorReady = false;
|
||||
Serial.print("|M1CalInit");
|
||||
}
|
||||
|
||||
if(motorReady)
|
||||
{
|
||||
movementState = cal_motor0;
|
||||
startMotorCalibration(MOTOR0);
|
||||
Serial.print("|MoCalStart");
|
||||
}
|
||||
}
|
||||
break;
|
||||
case cal_motor0:
|
||||
{
|
||||
Serial.print("|MoCalM0");
|
||||
e_calStates currentCalState = getCalibrationState(MOTOR0);
|
||||
if (currentCalState == calibrated || currentCalState == calibrationError)
|
||||
{
|
||||
movementState = cal_motor1;
|
||||
startMotorCalibration(MOTOR1);
|
||||
}
|
||||
calibrateMotor(MOTOR0);
|
||||
}
|
||||
break;
|
||||
case cal_motor1:
|
||||
{
|
||||
Serial.print("|MoCalM1");
|
||||
e_calStates currentCalState = getCalibrationState(MOTOR1);
|
||||
if (currentCalState == calibrated || currentCalState == calibrationError)
|
||||
{
|
||||
movementState = cal_done;
|
||||
}
|
||||
calibrateMotor(MOTOR1);
|
||||
}
|
||||
break;
|
||||
case cal_done:
|
||||
{
|
||||
Serial.print("|MoCalDone");
|
||||
disableProximity();
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void doMovement(void)
|
||||
{
|
||||
if (calibrateMovement())
|
||||
{
|
||||
//increaseMotor(MOTOR0);
|
||||
//decreaseMotor(MOTOR1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void initMovement( void )
|
||||
void handleMovement(void)
|
||||
{
|
||||
/* noting to init */
|
||||
|
||||
uint64_t currentmillis = millis();
|
||||
if (currentmillis - movementTimer1 > MOVEMENTINTERVAL)
|
||||
{
|
||||
Serial.print("|DoMov");
|
||||
doMovement();
|
||||
movementTimer1 = currentmillis;
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.print("|Mov");
|
||||
}
|
||||
}
|
||||
@@ -2,13 +2,21 @@
|
||||
|
||||
uint64_t proximityTimer = 0;
|
||||
uint64_t sampleTimer = 0;
|
||||
uint16_t readbuffer[ANALOGSAMPLES];
|
||||
uint16_t readbuffer[ANALOGSAMPLES] = {
|
||||
0,
|
||||
};
|
||||
uint16_t readbuffer_idx = 0;
|
||||
bool firstOutputSampleReady = false;
|
||||
uint16_t currentProx = 0;
|
||||
bool proxInitDone = false;
|
||||
bool proxEnabled = false;
|
||||
|
||||
uint32_t addSample(void)
|
||||
{
|
||||
if(!proxEnabled || (millis() - proximityTimer < 5))
|
||||
{
|
||||
return 0xffff;
|
||||
}
|
||||
uint64_t currentmillis = millis();
|
||||
if (currentmillis - sampleTimer > SAMPLERATE)
|
||||
{
|
||||
@@ -18,12 +26,8 @@ uint32_t addSample(void)
|
||||
readbuffer_idx = 0;
|
||||
firstOutputSampleReady = true;
|
||||
}
|
||||
digitalWrite(PROX_LED, HIGH);
|
||||
delay(3);
|
||||
readbuffer[readbuffer_idx++] = analogRead(PROX_IN);
|
||||
|
||||
digitalWrite(PROX_LED, LOW);
|
||||
|
||||
if (firstOutputSampleReady)
|
||||
{
|
||||
//average samples
|
||||
@@ -41,30 +45,40 @@ uint32_t addSample(void)
|
||||
return readbuffer[readbuffer_idx - 1];
|
||||
}
|
||||
|
||||
void enableProximity(void)
|
||||
{
|
||||
proxEnabled = true;
|
||||
digitalWrite(PROX_LED, HIGH);
|
||||
proximityTimer = millis();
|
||||
}
|
||||
|
||||
void disableProximity(void)
|
||||
{
|
||||
proxEnabled = false;
|
||||
digitalWrite(PROX_LED, LOW);
|
||||
}
|
||||
|
||||
void initProximity(void)
|
||||
{
|
||||
Serial.print("Proximity init:");
|
||||
pinMode(PROX_LED, OUTPUT);
|
||||
pinMode(PROX_IN, ANALOG);
|
||||
|
||||
for (auto sample : readbuffer)
|
||||
{
|
||||
sample = 0;
|
||||
}
|
||||
//pinMode(PROX_IN, ANALOG);
|
||||
proxInitDone = true;
|
||||
Serial.println(" OK ");
|
||||
}
|
||||
|
||||
void handleProximity(void)
|
||||
{
|
||||
uint64_t currentmillis = millis();
|
||||
if (currentmillis - proximityTimer > PROXIMITYINTERVAL)
|
||||
{
|
||||
//Serial.printf("proximity raw read = %u\n", currentProx);
|
||||
proximityTimer = currentmillis;
|
||||
}
|
||||
|
||||
Serial.print("|Pr");
|
||||
addSample();
|
||||
}
|
||||
void getProximity(void)
|
||||
|
||||
uint16_t getProximity(void)
|
||||
{
|
||||
return currentProx;
|
||||
}
|
||||
|
||||
bool getProxInitState(void)
|
||||
{
|
||||
return proxInitDone;
|
||||
}
|
||||
Reference in New Issue
Block a user