From 92c7d125fdba419ac9f6bd952e3173f054595d5e Mon Sep 17 00:00:00 2001 From: Fisch Date: Tue, 7 May 2019 10:05:11 +0200 Subject: [PATCH] move esp motor controller code to arduino --- .../mixercontroller_w5100.ino | 234 ++++++++++++++++-- 1 file changed, 217 insertions(+), 17 deletions(-) diff --git a/controller/mixercontroller_w5100/mixercontroller_w5100.ino b/controller/mixercontroller_w5100/mixercontroller_w5100.ino index f15f77a..cf2b410 100644 --- a/controller/mixercontroller_w5100/mixercontroller_w5100.ino +++ b/controller/mixercontroller_w5100/mixercontroller_w5100.ino @@ -7,32 +7,103 @@ #include "Ethernet.h" #include "PubSubClient.h" +//Ethernet and MQTT String ip = ""; uint8_t mac[6] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x06}; -#define CLIENT_ID "Hal" +#define CLIENT_ID "Mixer" EthernetClient ethClient; PubSubClient mqttClient; #define PUBLISH_DELAY 10000 long last_send=0; + + +//Serial +long last_serialdebug=0; +#define INTERVAL_SERIALDEBUG 200 + +//Inputs +#define PIN_BUTTON 9 +#define PIN_ENCA 7 +#define PIN_ENCB 8 +#define BUTTON_RELEASE_DEBOUNCE 100 //minimum time after button release to reenable triggering + +boolean button_flag=false; //true if button pressed +boolean button_released=true; +long last_button_released=0; //last time button has been released (for debounce) + +//Shift Register 595 +//connections: https://www.arduino.cc/en/tutorial/ShiftOut +#define SRLATCH 4 +#define SRCLOCK 5 +#define SRDATA 6 +uint16_t srbits=0; + +#include +Encoder volEnc(PIN_ENCA,PIN_ENCB); +float encoderMultiplier=4.0; + +//Servo stuff +#define SRPIN_MOTOR_IN1 1 //L293(pin2) Motor IN1 +#define SRPIN_MOTOR_IN2 2 //L293(pin7) Motor IN2 + +#define PIN_POT A0 //reference potentiometer wiper +#define DEADZONE_POTI 5 //maximum allowed error. stop when reached this zone +#define POT_MIN 10 //minimum value pot can reach +#define POT_MAX 1010 //maximum value pot can reach +#define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever + +int poti_set=512; //set value +int poti_read=0; //read value from poti +boolean poti_reachedposition=true; //set to true if position reached. after that stop turning + +#define MOTOR_STOP(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,LOW); +#define MOTOR_LEFT(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,HIGH); +#define MOTOR_RIGHT(); srWrite(SRPIN_MOTOR_IN1,HIGH); srWrite(SRPIN_MOTOR_IN2,LOW); +#define MOTOR_TURNING() (srRead(SRPIN_MOTOR_IN1) != srRead(SRPIN_MOTOR_IN2)) + + +//Motorcheck +long last_motorcheck=0; +#define INTERVAL_MOTORCHECK 100 //check motor movement every x ms +int poti_read_last=0; +int motor_vel=0; //analog read units per second +#define MINIMUM_MOTORVEL 20 //minimum velocity motor should turn wenn active +#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate +long last_motorTooSlow=0; //typically 0 + +//error +uint8_t error=0; +#define NOERROR 0 +#define MOTORDIDNOTTURN 1 + void setup() { pinMode(LED_BUILTIN, OUTPUT); digitalWrite(LED_BUILTIN,HIGH); - // setup serial communication + pinMode(PIN_BUTTON,INPUT_PULLUP); + + pinMode(PIN_POT,INPUT); + + pinMode(SRLATCH, OUTPUT); + pinMode(SRCLOCK, OUTPUT); + pinMode(SRDATA, OUTPUT); + Serial.begin(9600); while (!Serial) {}; - Serial.println(F("MQTT Arduino Demo")); + Serial.println("Booting"); - // setup ethernet communication using DHCP - if (Ethernet.begin(mac) == 0) { - //Serial.println(F("Unable to configure Ethernet using DHCP")); + + if (Ethernet.begin(mac) == 0) { // setup ethernet communication using DHCP + Serial.println("Unable to configure Ethernet using DHCP"); + Serial.flush(); + delay(200); for (;;); } - Serial.println(F("Ethernet configured via DHCP")); + Serial.println("Ethernet configured via DHCP"); Serial.print("IP address: "); Serial.println(Ethernet.localIP()); Serial.println(); @@ -49,12 +120,11 @@ void setup() { // setup mqtt client mqttClient.setClient(ethClient); mqttClient.setServer("10.0.0.1", 1883); - Serial.println(F("MQTT client configured")); + Serial.println("MQTT client configured"); mqttClient.setCallback(callback); - Serial.println(); - Serial.println(F("Ready to send data")); + Serial.println("Ready"); last_send = millis(); @@ -62,7 +132,7 @@ void setup() { } void reconnect() { - // Loop until we're reconnected + // Loop until reconnected while (!mqttClient.connected()) { Serial.print("Attempting MQTT connection..."); // Attempt to connect @@ -74,23 +144,136 @@ void reconnect() { Serial.print("failed, rc="); Serial.print(mqttClient.state()); Serial.println(" try again in 5 seconds"); - // Wait 5 seconds before retrying - delay(5000); + delay(5000);// Wait 5 seconds before retrying } } } void loop() { + long loopmillis=millis(); if (!mqttClient.connected()) { reconnect(); } + mqttClient.loop(); + - if (millis() - last_send > PUBLISH_DELAY) { - //sendData(); - last_send = millis(); + + //Serial Input ############################################## + while (Serial.available() > 0) { + int _value = Serial.parseInt(); + if (Serial.read() == '\n') { + Serial.print("value="); + Serial.println(_value); + //poti_set=_value; + //poti_reachedposition=false; //aim for new position + srWrite(_value,!srRead(_value)); + + } + } - mqttClient.loop(); + //Inputs ################################################### + poti_read=poti_read*POTIFILTER + (1.0-POTIFILTER)*analogRead(PIN_POT); //read poti + + if (!digitalRead(PIN_BUTTON)){ //button pressed + if (button_released){ + button_released=false; //flag: not released + if(loopmillis-last_button_released > BUTTON_RELEASE_DEBOUNCE){ + button_flag=true; + } + } + }else if(!button_flag && !button_released){ //button released and flag has been cleared + last_button_released=loopmillis; + button_released=true; + } + + //Read Encoder to velocity "volEncVel" + int volEncVel=0; + int _volEnc=volEnc.read(); + if (_volEnc!=0){ //encoder moved + volEncVel=_volEnc; + volEnc.write(0); //reset value + } + + + //Input Handling + if (volEncVel!=0){ //knob moved + poti_set+=volEncVel*encoderMultiplier; //change poti set value + poti_set=constrain(poti_set, POT_MIN,POT_MAX); + poti_reachedposition=false; + } + + + //Motor Movement Routine ################# + if (error==0){ //no errors + + if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //error too high + digitalWrite(LED_BUILTIN,HIGH); + if (poti_read-poti_set < 0){ + MOTOR_LEFT(); + }else{ + MOTOR_RIGHT(); + } + }else if(!poti_reachedposition){ //position reached but flag not set + MOTOR_STOP(); + Serial.print("reached:"); + Serial.print(" set="); + Serial.print(poti_set); + Serial.print(" is="); + Serial.print(poti_read); + Serial.print(" vel="); + Serial.println(); + poti_reachedposition=true; //position reached + digitalWrite(LED_BUILTIN,LOW); + } + + + if ( loopmillis > last_motorcheck+INTERVAL_MOTORCHECK){ + last_motorcheck=loopmillis; + + motor_vel=(poti_read-poti_read_last)*1000 /INTERVAL_MOTORCHECK ; //calculate current motor velocity + poti_read_last=poti_read; + + //motor fail check + if (MOTOR_TURNING() && abs(motor_vel) MOTOR_FAILTIME){ + error=MOTORDIDNOTTURN; + Serial.println("MOTORDIDNOTTURN"); + } + + }else if (last_motorTooSlow>0){ //was recognized too slow but is now turning fast again + last_motorTooSlow=0; //reset + } + + } + }else{ //an error occured. error!=0 + MOTOR_STOP(); + } + + + + if ( loopmillis > last_serialdebug+INTERVAL_SERIALDEBUG){ + last_serialdebug=loopmillis; + + + Serial.print(" set="); + Serial.print(poti_set); + Serial.print(" is="); + Serial.print(poti_read); + Serial.print(" vel="); + Serial.print(motor_vel); + Serial.println(""); + + if (button_flag){ //TODO: remove hier if correct behaviour implemented + Serial.println("BUTTON Pressed"); + button_flag=false; //clear flag to reenable button triggering. + } + + } + + } void sendData() { @@ -129,3 +312,20 @@ void callback(char* topic, byte* payload, unsigned int length) { } + + + +void srWrite(uint8_t pin, boolean state){ + if (state==true){ + srbits |= 1UL << pin; //set bit + }else{ + srbits &= ~(1UL << pin); //clear bit + } + digitalWrite(SRLATCH, LOW); + shiftOut(SRDATA, SRCLOCK, MSBFIRST, srbits>>8); + shiftOut(SRDATA, SRCLOCK, MSBFIRST, srbits); + digitalWrite(SRLATCH, HIGH); +} +boolean srRead(uint8_t pin){ + return (srbits >> pin) & 1U; +}