updated code and sch

CAD/PCB/clockclock-cache.lib

@@ -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
+DEF Diode_1N4448WS D 0 40 N N 1 F N
-F0 "D" 25 -100 50 H V L CNN
+F0 "D" 0 100 50 H V C CNN
-F1 "Diode_BAV70" 0 100 50 H V C CNN
+F1 "Diode_1N4448WS" 0 -100 50 H V C CNN
-F2 "Package_TO_SOT_SMD:SOT-23" 0 0 50 H I 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 10 -50 50 -50 -50 N
-P 2 0 1 0 -150 0 150 0 N
+P 2 0 1 0 50 0 -50 0 N
-P 2 0 1 0 0 0 0 -100 N
+P 4 0 1 10 50 50 50 -50 -50 0 50 50 N
-P 3 0 1 10 -50 -50 -50 50 -50 50 N
+X K 1 -150 0 100 R 50 50 1 1 P
-P 3 0 1 10 50 -50 50 50 50 50 N
+X A 2 150 0 100 L 50 50 1 1 P
-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
 ENDDRAW
 ENDDEF
 #

clockclock/clockclock.code-workspace

@@ -4,5 +4,13 @@
 			"path": "."
 		}
 	],
-	"settings": {}
+	"settings": {
+		"files.associations": {
+			"*.tcc": "cpp",
+			"fstream": "cpp",
+			"istream": "cpp",
+			"ostream": "cpp",
+			"sstream": "cpp"
+		}
+	}
 }

clockclock/include/board.h

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

clockclock/platformio.ini

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

clockclock/src/Proximity.h

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

clockclock/src/board.cpp

@@ -3,4 +3,5 @@
 void initBoard( void )
 {
   Serial.begin(115200);
+  delay(500);
 }

clockclock/src/buttons.cpp

@@ -6,6 +6,7 @@ Button buttonMin(MCU_BUT_MIN, 25UL, true, true);
 
 void handleButtons(void)
 {
+  Serial.print("|Bu");
   if (buttonPlus.read())
   {
     increaseMotor(MOTOR0);

clockclock/src/comms.cpp

@@ -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
+  if (address == COMMSBASE)
-  Wire.begin(address); 
+  {
-  // Attach a function to trigger when something is received.
+    //init as master
-  //Wire.onReceive(receiveI2CEvent);
+    initCommsMaster();
-  //Wire.onRequest(requestI2CEvent);
+  }
+  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
+}

clockclock/src/comms.h

@@ -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];
+  byte _dataPacket[11] = {
-  uint8_t _bufIndex = 0;
+      0,
-  bool validheader;
+  };
+  byte _lastMasterCommand = 0;
+  byte _command;
+  int _slaveID;
+
 public:
-  c_message();
+  c_message(void) {}
-
+  void makeDataPacket(uint16_t aa, uint16_t bb, uint16_t cc, uint16_t dd, uint16_t ee);
-  void addByte(uint8_t byte)
+  void sendDataPacket();
-  {
+  int sumFunction(int aa, int bb, int cc, int dd, int ee);
-    _buffer[_bufIndex++] = byte;
-    if(_bufIndex >= MSGLEN)
-    {
-      _bufIndex = 0;
-    }
-    
-  }
 
+  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 receiveDataPacket(int howMany);
-void handleComms ( void );
+void slavesRespond();
+
+void initComms(void);
+void handleComms(void);
+
+void initCommsMaster(void);
+void initCommsSlave(int SlaveDeviceId);

clockclock/src/led.cpp

@@ -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;
+	void fadeall() { for(int i = 0; i < NUM_LEDS; i++) { leds[i].nscale8(250); } }
-uint8_t hue = 0;
+#endif
-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);
 
 
 
@@ -25,20 +19,55 @@ RgbColor black(0);
 void initLeds(void)
 {
   Serial.print("Init Leds: ");
-  LEDS.begin();
+#ifdef HASLEDS
-  LEDS.ClearTo(black);
+	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 
 }

clockclock/src/led.h

@@ -1,8 +1,10 @@
 #pragma once
 
 #include "board.h"
-#include <FastLED.h>
+#ifdef HASLEDS
-#include <NeoPixelBus.h>
+  #define FASTLED_INTERNAL
+  #include <FastLED.h>
+#endif
 
 #define NUM_LEDS 4
 #define LEDINTERVAL 10

clockclock/src/main.cpp

@@ -15,16 +15,46 @@ void setup() {
   initMovement();
   initProximity();
   initLeds();
-  initComms();
+  //initComms();
-  
 }
 
-void loop() {
+void handle1msTasks(void)
-  // put your main code here, to run repeatedly:
+{
   handleMotors();
   handleButtons();
-  handleMovement();
   handleProximity();
+  handleMovement();
+}
+
+void handle10msTasks(void)
+{
+
   handleLeds();
-  handleComms();
+}
+
+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");
+  }
+  
 }

clockclock/src/motor.cpp

@@ -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);
+std::vector<c_motor *> motor_list;
-c_motor motor1(MOT_STCK1, MOT_DIR1, MOT_EN, MOT_RST1);
 
-MultiStepper motors;
+bool setvref = false;
 
-void c_motor::init(bool setvref)
+void c_motor::init()
 {
-  Serial.print("init Motors: ");
+  Serial.printf("init %s: ", _motorName);
-  pinMode(_rst, OUTPUT);
+#if HARDWAREVERSION == 12
-  pinMode(_en, OUTPUT);
+  pinMode(_pin1, OUTPUT);
-  pinMode(_sclk, OUTPUT);
+  pinMode(_pin2, OUTPUT);
-  pinMode(_dir, OUTPUT);
+  pinMode(_pin3, OUTPUT);
+#else
+  pinMode(_pin3, OUTPUT);
+  pinMode(_pin4, OUTPUT);
+#endif
 
-  digitalWrite(_rst, 0);
+  digitalWrite(_pin3, 0); //rst
-  digitalWrite(_sclk, 0);
+  digitalWrite(_pin4, 0); //en
-  digitalWrite(_dir, 0);
-  digitalWrite(_en, 0);
 
   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); 
+    dacWrite(MOT_VREF, 196);
-    //pinMode(MOT_VREF, OUTPUT);
+    setvref = true;
-    ledcAttachPin(MOT_VREF,0);
-    ledcWrite(MOT_VREF, 32);
   }
-  Serial.println("OK");
+#endif
+  _initDone = true;
+  Serial.print("OK");
 }
 
 void c_motor::begin(void)
 {
-  _stepper.begin(RPM, MICROSTEPS);
+  _stepper.setMaxSpeed(MOTORMAXSPEED);
-  _stepper.enable();
+  _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();
+  Serial.print("|Mot");
-  motor1.run();
+  for (auto &&thismotor : motor_list)
+  {
+    thismotor->run();
+  }
 }
 
 void initMotors(void)
 {
-  motor0.init(true);
+  Serial.print("init motors:");
-  motor0.begin();
+  motor_list.clear();
-
+  motor_list.push_back(&motor0);
-  motor1.init();
+  motor_list.push_back(&motor1);
-  motor1.begin();
+  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();
+    Serial.print("|IncrNULLPTR");
-  }
+    return;
-  if (motor == MOTOR1)
-  {
-    motor1.increasePos();
   }
+  thismotor->increasePos();
 }
 
 void decreaseMotor(e_motor motor)
 {
-  if (motor == MOTOR0)
+  c_motor *thismotor = getMotor(motor);
+  if (thismotor == NULL)
   {
-    motor0.decreasePos();
+    Serial.print("|DecrNULLPTR");
+    return;
   }
-  if (motor == MOTOR1)
+  thismotor->decreasePos();
+}
+
+bool motorsCalibrated(void)
+{
+  for (auto &&thismotor : motor_list)
   {
-    motor1.decreasePos();
+    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();
+}

clockclock/src/motor.h

@@ -1,65 +1,100 @@
 #pragma once
 
-//#include "AccelStepper.h"
+#include "AccelStepper.h"
-#include "BasicStepperDriver.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;
+  const e_motor _index;
-  long _targetpos;
+  const int _pin1; //sclk;
+  const int _pin2; //_dir;
+  const int _pin3; //_en;
+  const int _pin4; //_rst;
 
-  //AccelStepper _stepper;
+  const char *const _motorName;
-  BasicStepperDriver _stepper;
+
+  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),
+  c_motor(e_motor index, int sclk, int dir, int en, int rst, const char *name)
-                                                _stepper(MOTOR_STEPS, _sclk, _dir, _en)
+        : _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);
-  void moveTo(long pos)
+  void move(long pos);
-  {
+  bool ready(void);
-    _stepper.startMove(pos);
+  void run(void);
-    if((_targetpos += pos) > 360)
-    {
-      _targetpos -= 360;
-    }
-  }
-
-  bool ready( void );
-  void run(void)
-  {
-    _stepper.nextAction();
-
-  }
-
   void increasePos(void);
   void decreasePos(void);
-
+  AccelStepper *getStepper(void) { return &_stepper; }
-  BasicStepperDriver *getStepper(void)
+  bool getIndex(e_motor index);
-  {
+  bool isCalibrated(void);
-    return &_stepper;
+  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);

clockclock/src/movement.cpp

@@ -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);
+  Serial.print("Init Movement:");
-  decreaseMotor(MOTOR1);
+  Serial.print("check calibration:");
+  /* noting to init */
+  motorsCalibrated();
+  Serial.println(" OK");
 }
 
-void handleMovement( void )
+enum calibrationstates
 {
-  uint64_t currentmillis = millis();
+  cal_idle,
-  if(currentmillis - movementTimer1 > MOVEMENTINTERVAL)
+  cal_init,
+  cal_motor0,
+  cal_motor1,
+  cal_done
+};
+
+calibrationstates movementState = cal_idle;
+
+bool calibrateMovement(void)
+{
+  switch (movementState)
   {
-    doMovement();
+  case cal_idle:
-    movementTimer1 = currentmillis;
+  {
+    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 handleMovement(void)
-void initMovement( void )
 {
-  /* noting to init */
+
+  uint64_t currentmillis = millis();
+  if (currentmillis - movementTimer1 > MOVEMENTINTERVAL)
+  {
+    Serial.print("|DoMov");
+    doMovement();
+    movementTimer1 = currentmillis;
+  }
+  else
+  {
+    Serial.print("|Mov");
+  }
 }

clockclock/src/proximity.cpp

@@ -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);
+  //pinMode(PROX_IN, ANALOG);
-
+  proxInitDone = true;
-  for (auto sample : readbuffer)
-  {
-    sample = 0;
-  }
   Serial.println(" OK ");
 }
 
 void handleProximity(void)
 {
-  uint64_t currentmillis = millis();
+  Serial.print("|Pr");
-  if (currentmillis - proximityTimer > PROXIMITYINTERVAL)
-  {
-    //Serial.printf("proximity raw read = %u\n", currentProx);
-    proximityTimer = currentmillis;
-  }
-
   addSample();
 }
-void getProximity(void)
+
+uint16_t getProximity(void)
 {
+  return currentProx;
+}
+
+bool getProxInitState(void)
+{
+  return proxInitDone;
 }