Close

Code

A project log for OpenOpener

Open source (software and hardware) DC motor controller for electric gates

manuManu 05/03/2015 at 21:336 Comments

Well, I never posted any code for that project, because I forgot it, let's correct that:

Arduino Pro Mini 5V w/ ATmega328


/* 
  OpenOpener :
  dual swing-gate automatic gate opener
*/

// pins definitions :
// D0  - USB Rx               shunt M1 - A0  
// D1  - USB Tx               shunt M2 - A1
// D2  - BP stop                       - A2
// D3  - Remote Control                - A3                                 
// D4  - Relay M1A         Rpi i2c SDA - A4
// D5  - Relay M1B         Rpi i2c SCL - A5
// D6  - Relay M2A       
// D7  - Relay M2B
//
// D8  - BP close
// D9  - BP open
// D10 - PWM M1 
// D11 - PWM M2
// D12 - relay FlashLight
// D13 -
#define PIN_RELAY_M1A  4  // D3 relay board
#define PIN_RELAY_M1B  5  // D2 relay board
#define PIN_RELAY_M2A  6  // D1 relay board
#define PIN_RELAY_M2B  7  // D0 relay board

#define PIN_BP_STOP  2  // pin 2 for external interrupt
#define PIN_BP_OPEN  9
#define PIN_BP_CLOSE 8

#define PIN_REMOTE 3

#define PIN_PWM_M1 10
#define PIN_PWM_M2 11

#define PIN_FLASHLIGHT 12

#define PIN_CURRENT_M1 0  // analog A0
#define PIN_CURRENT_M2 1  // analog A1

// defines
//#define _DEBUG_BP
#undef _DEBUG_BP

// parameters:
#define DELAYINMILLISEC 4000
#define TIMEOUTMOTORINMILLISEC /*5000*/30000
#define TIME_INHIBIT_IMES_INMILLISEC 1000
#define MAX_OVERCURRENT_IN_AMP  3.0f
#define MIN_MOTOR_CURRENT_IN_AMP 0.2f  // minimum value when a motor spins (failure sensor detection) 
#define IMESARRAY_SIZE 10

// enum
enum OpeningStates {
  INIT_OPEN,
  OPEN_M1,
  OPEN_DELAY,
  OPEN_M2,
  WAIT_OPEN_END,
  END_OPEN
};

enum ClosingStates {
  INIT_CLOSE,
  CLOSE_M1,
  CLOSE_DELAY,
  CLOSE_M2,
  WAIT_CLOSE_END,
  END_CLOSE
};

enum RemoteStates {
  STATE_STOP1,  // 0
  STATE_OPEN,   // 1    
  STATE_STOP2,  // 2
  STATE_CLOSE   // 3
};

// global variables
unsigned char StopCount;
unsigned char nextState;
boolean bOpening;  // flag for opening gates
boolean bClosing;  // flag for closing gates
unsigned long timeStartOpenM1 = 0;
unsigned long timeStartOpenM2 = 0;
unsigned long timeStartCloseM2 = 0;
unsigned long timeStartCloseM1 = 0;
boolean bStopNow = false;
float currentM1_inAmp = 0.0f;
float currentM2_inAmp = 0.0f;
unsigned char currentState = STATE_STOP1;  // remote control state machine
unsigned char newState = STATE_STOP1;  // remote control state machine;
unsigned long lastTimeCheck = 0;
unsigned long timeCurrent = 0;  // check currents values from shunt resistors
int ImesCount = 0;
int I1array[IMESARRAY_SIZE] = {0};
int I2array[IMESARRAY_SIZE] = {0};
int modeM1 = 0;
int modeM2 = 0;
bool isM1Stopped = false;
bool isM2Stopped = false;
unsigned long timeStartFlashLight = 0;
boolean FlashLightState = false;

// setup:
void setup() {
  // init globals
  StopCount = 0;
  nextState = 0;
  bOpening = false;
  bClosing = false;
  
  // output pins:
  pinMode(PIN_RELAY_M1A, OUTPUT); digitalWrite(PIN_RELAY_M1A, LOW);
  pinMode(PIN_RELAY_M1B, OUTPUT); digitalWrite(PIN_RELAY_M1B, LOW);
  pinMode(PIN_RELAY_M2A, OUTPUT); digitalWrite(PIN_RELAY_M2A, LOW);
  pinMode(PIN_RELAY_M2B, OUTPUT); digitalWrite(PIN_RELAY_M2B, LOW);
  pinMode(PIN_FLASHLIGHT, OUTPUT), digitalWrite(PIN_FLASHLIGHT, LOW);
  
  // input pins:
  pinMode(PIN_BP_OPEN, OUTPUT); digitalWrite(PIN_BP_OPEN, LOW);
  pinMode(PIN_BP_CLOSE, OUTPUT); digitalWrite(PIN_BP_CLOSE, LOW);
  pinMode(PIN_BP_STOP, OUTPUT); digitalWrite(PIN_BP_STOP, LOW);
  pinMode(PIN_REMOTE, INPUT);
  
  // Serial link
  Serial.begin(9600);
  Serial.println("OpenOpener");
  
  // interrupt pin for "remote control" input pin
  attachInterrupt(1, RemoteEvent, RISING);
  
  StopGates();
}

// loop:
void loop() {
  // current acquisition every 10ms:
  if( (millis()-timeCurrent)>=10 ) {
    timeCurrent = millis(); // reinit current chrono
    
    I1array[ImesCount] = analogRead(PIN_CURRENT_M1);
   
    I2array[ImesCount] = analogRead(PIN_CURRENT_M2);
    
    ImesCount++;
    if(ImesCount >= 10) {
      ImesCount = 0;
        
      // median filter
      isort(I1array, IMESARRAY_SIZE);
      modeM1 = mode(I1array, IMESARRAY_SIZE);
      currentM1_inAmp = (float)(modeM1)*5.0f/1024.0;  // 1024 <-> 5V=5A
      
      isort(I2array, IMESARRAY_SIZE);
      modeM2 = mode(I2array, IMESARRAY_SIZE);
      currentM2_inAmp = (float)(modeM2)*5.0f/1024.0;  // 1024 <-> 5V=5A
    }
  }
  
  // read buttons:
  boolean bStop = false/*digitalRead(PIN_BP_STOP)*/;
  boolean bOpen = false/*digitalRead(PIN_BP_OPEN)*/;
  boolean bClose = false/*digitalRead(PIN_BP_CLOSE)*/;

#ifdef _DEBUG_BP  
  // debug:
  unsigned long static waitalittle;
  if(millis()-waitalittle > 500) {
    waitalittle = millis();
    Serial.print("bStopNow:"); Serial.print(bStopNow); Serial.print("; bOpening:"); Serial.print(bOpening); Serial.print("; bClosing:"); Serial.println(bClosing);
  }
#endif // _DEBUG_DP  
  
  // TODO: debounce buttons
  
  // remote control read:
  if(newState != currentState) {  // remote control detection
    switch(newState) {
      case STATE_STOP1:
      case STATE_STOP2:
        bStop = true;
        bStopNow = true;
      break;
      case STATE_OPEN:
        bOpening = true;
        nextState = INIT_OPEN;
      break;
      case STATE_CLOSE:
        bClosing = true;
        nextState = INIT_CLOSE;
      break;
      default: bStopNow = true;
    }
    currentState = newState;
    Serial.print("current state:"); Serial.println(currentState);
    Serial.print("bOpen:"); Serial.print(bOpen); Serial.print("; bClose:");Serial.println(bClose);
    Serial.print("bStopNow:"); Serial.print(bStopNow); Serial.print("; bOpening:"); Serial.print(bOpening); Serial.print("; bClosing:"); Serial.println(bClosing); Serial.println("");
  }
  
  // set flag for state machines
  if(bOpen && !bStopNow && !bOpening && !bClosing) { 
    bOpening = true;  // flag to start opening process
    nextState = INIT_OPEN;
    newState = STATE_OPEN;
  }
  else if(bClose && !bStopNow && !bClosing && !bOpening) {
    bClosing = true;  // flag to start closing process
    nextState = INIT_CLOSE;
    newState = STATE_CLOSE;
  }
  
  // state machines
  if(!bStopNow ) {  // until a stop condition appears
    if(bOpening) {
      nextState = fsmOpen(nextState);
      if(nextState == END_OPEN) {
        StopGates();  // stop
        bOpening = false;  // end of opening process state machine
        newState = STATE_STOP2;
        SendMessage("End of opening process.");
      }
    }
    else if(bClosing) {
      nextState = fsmClose(nextState);
      if(nextState == END_CLOSE) {
        StopGates();  // stop
        bClosing = false;  // end of opening process state machine
        newState = STATE_STOP1;
        SendMessage("End of closing process.");
      }
    }
  }
  else {
    StopGates();    
  }
  
  if(bStopNow) {  // press "open" or "close" button to release the "stop" state
    StopGates();
    bStopNow = false;
    bOpening = false;
    bClosing = false;
    if(currentState == STATE_OPEN) { newState = STATE_STOP2; }
    else if (currentState == STATE_CLOSE) { newState = STATE_STOP1; }
    else { newState = currentState; }
    SendMessage("StopEvent");
    Serial.print("Stop count: "); Serial.println(StopCount); Serial.println("");
  }
}

void StopGates() {
  // stop the gates
  digitalWrite(PIN_RELAY_M1A, LOW);
  digitalWrite(PIN_RELAY_M1B, LOW);
  digitalWrite(PIN_RELAY_M2A, LOW);
  digitalWrite(PIN_RELAY_M2B, LOW);
  isM1Stopped = true;
  isM2Stopped = true;
  ResetFlashLight();
  SendMessage("stop");
}

void StopGateM1() {
  // stop the gate M1
  digitalWrite(PIN_RELAY_M1A, LOW);
  digitalWrite(PIN_RELAY_M1B, LOW);
  isM1Stopped = true;
  SendMessage("stop M1");
}

void StopGateM2() {
  // stop the gate M2
  digitalWrite(PIN_RELAY_M2A, LOW);
  digitalWrite(PIN_RELAY_M2B, LOW);
  isM2Stopped = true;
  SendMessage("stop M2");
}

void OpenGateM1() {
  // open gate motor M1
  digitalWrite(PIN_RELAY_M1A, HIGH);
  digitalWrite(PIN_RELAY_M1B, LOW);
  isM1Stopped = false;
  SendMessage("opening 1st gate...");
}

void OpenGateM2() {
  // open gate motor M2s
  digitalWrite(PIN_RELAY_M2A, HIGH);
  digitalWrite(PIN_RELAY_M2B, LOW);
  isM2Stopped = false;
  SendMessage("opening 2nd gate...");
}

void CloseGateM1() {
  // close gate motor M1
  digitalWrite(PIN_RELAY_M1A, LOW);
  digitalWrite(PIN_RELAY_M1B, HIGH);
  isM1Stopped = false;
  SendMessage("closing 1st gate...");
}

void CloseGateM2() {
  // close gate motor M2
  digitalWrite(PIN_RELAY_M2A, LOW);
  digitalWrite(PIN_RELAY_M2B, HIGH);
  isM2Stopped = false;
  SendMessage("closing 2nd gate...");
}

void SendMessage(char *Message) {
  Serial.println(Message);
}

void StopEvent() {
  StopCount++;
  bStopNow = true;  // memorize "stop" button
}

void RemoteEvent() {
  switch(currentState) {
    case STATE_STOP1: newState = STATE_OPEN;  break;  // from stop to open
    case STATE_OPEN:  newState = STATE_STOP2; break;  // from open to stop
    case STATE_STOP2: newState = STATE_CLOSE; break;  // from stop to close
    case STATE_CLOSE: newState = STATE_STOP1; break;  // from close to stop
    default: newState = STATE_STOP1;
  }
//  SendMessage("Remote Control event.\r\n");
}

unsigned char fsmOpen(unsigned char state) {
  unsigned char returnState = END_OPEN;  // default
  
  // check overcurrent at every iteration to stop gate in case of obstacle
  // M1 overcurrent test:
  if( (state > OPEN_M1) && ( (millis()-timeStartOpenM1) >= TIME_INHIBIT_IMES_INMILLISEC && (isM1Stopped == false) ) ) {  //1s inhibit current measure
    if(currentM1_inAmp > MAX_OVERCURRENT_IN_AMP) {
      char msg[40];
      int n = sprintf(msg, "M1 overcurrent detect: ");
      dtostrf(currentM1_inAmp, 3, 1, msg+n);
      SendMessage(msg);
      StopGateM1();  // halt M1
    }
    else if(currentM1_inAmp < MIN_MOTOR_CURRENT_IN_AMP && (isM1Stopped == false) ) {
      // no current detection : maybe a sensor failure => STOP
      SendMessage("No M1 current value!");
    }
  }
  // M2 overcurrent test:
  if( (state > OPEN_M2 ) && ( (millis()-timeStartOpenM2) >= TIME_INHIBIT_IMES_INMILLISEC && (isM2Stopped == false) ) ) {  //1s inhibit current measure
    if(currentM2_inAmp > MAX_OVERCURRENT_IN_AMP) {
      char msg[40];
      int n = sprintf(msg, "M2 overcurrent detect: ");
      dtostrf(currentM2_inAmp, 3, 1, msg+n);
      SendMessage(msg);
      StopGateM2();  // halt M2
    }
    else if(currentM2_inAmp < MIN_MOTOR_CURRENT_IN_AMP && (isM2Stopped == false) ) {
      // no current detection : maybe a sensor failure => STOP
      SendMessage("No M2 current value!");
    }
  }
  // stop if needed:
  if(state == END_OPEN) {
    StopGates();
    return END_OPEN;  // exit fsm
  }
  
  switch(state) {
    
    case INIT_OPEN:
      SendMessage("Start Opening process...");
      timeStartOpenM1 = 0;
      timeStartOpenM2 = 0;
      lastTimeCheck = millis();
	  SetFlashLight();
      returnState = OPEN_M1;  // next state
    break;
    
    case OPEN_M1:
      timeStartOpenM1 = millis();  // memorize date for opening M1
      OpenGateM1();  // open first gate
      returnState = OPEN_DELAY;  // next state
    break;
    
    case OPEN_DELAY:
      if( (millis() - timeStartOpenM1) >=  DELAYINMILLISEC ) {
        returnState = OPEN_M2;  // next state
      }
      else {
        returnState = OPEN_DELAY;  // stay in delay
      }
    break;
    
    case OPEN_M2:
      OpenGateM2();
      timeStartOpenM2 = millis();  // memorize date for opening M2
      returnState = WAIT_OPEN_END;
    break;
    
    case WAIT_OPEN_END:
      if( (millis() - timeStartOpenM1) >=  TIMEOUTMOTORINMILLISEC ) {  // time elapsed since M1 start
        returnState = END_OPEN;  // next state
      }
      else {
        if(isM1Stopped && isM2Stopped) {  // both motors are stopped, np need to wait more
          returnState = END_OPEN;  // next state
        }
        else {
          returnState = WAIT_OPEN_END;  // stay in delay
        }
      }
    break;
    
    case END_OPEN:
      StopGates();
      returnState = END_OPEN;
    break;
    
    default:
      StopGates();  // stop when default
      returnState = END_OPEN;
  }
  
  if( (millis()-lastTimeCheck) >= 1000 )
  {
    lastTimeCheck = millis();
    Serial.print("Opening time: "); Serial.print( (millis()-timeStartOpenM1)/1000 );  // time since the begining of the opening fsm
    Serial.print("; Current M1 = "); Serial.print(currentM1_inAmp); Serial.print("\tCurrent M2 = "); Serial.println(currentM2_inAmp);
  }
  
  return returnState;
}

unsigned char fsmClose(unsigned char state) {
  unsigned char returnState = END_CLOSE;

  // check overcurrent at every iteration to stop gate in case of obstacle
  // M2 overcurrent test:
  if( (state > CLOSE_M2 ) && ( (millis()-timeStartCloseM2) >= TIME_INHIBIT_IMES_INMILLISEC && (isM2Stopped == false) ) ) {  //1s inhibit current measure
    if(currentM2_inAmp > MAX_OVERCURRENT_IN_AMP) {
      char msg[40];
      int n = sprintf(msg, "M2 overcurrent detect: ");
      dtostrf(currentM2_inAmp, 3, 1, msg+n);
      SendMessage(msg);
      StopGateM2();  // halt motor
    }
    else if(currentM2_inAmp < MIN_MOTOR_CURRENT_IN_AMP && (isM2Stopped == false) ) {
      // no current detection : maybe a sensor failure => STOP
      SendMessage("No M2 current value!");
    }
  }
  // M1 overcurrent test:
  if( (state > CLOSE_M1 ) && ( (millis()-timeStartCloseM1) >= TIME_INHIBIT_IMES_INMILLISEC && (isM1Stopped == false) ) ) {  //1s inhibit current measure
    if(currentM1_inAmp > MAX_OVERCURRENT_IN_AMP) {
      char msg[40];
      int n = sprintf(msg, "M1 overcurrent detect: ");
      dtostrf(currentM1_inAmp, 3, 1, msg+n);
      SendMessage(msg);
      StopGateM1();  // halt motors
    }
    else if(currentM1_inAmp < MIN_MOTOR_CURRENT_IN_AMP && (isM1Stopped == false) ) {
      // no current detection : maybe a sensor failure => STOP
      SendMessage("No M1 current value!");
    }
  }
  // stop if needed:
  if(state == END_CLOSE) {
    StopGates();
    return END_CLOSE;  // exit fsm
  }  
  
  switch(state) {
    
    case INIT_CLOSE:
      SendMessage("Start Closing process...");
      timeStartCloseM2 = 0;
      lastTimeCheck = millis();
	  SetFlashLight();
      returnState = CLOSE_M2;  // next state
    break;
    
    case CLOSE_M2:
      timeStartCloseM2 = millis();  // memorize date for closing M2
      CloseGateM2();  // close first gate
      returnState = CLOSE_DELAY;  // next state
    break;
    
    case CLOSE_DELAY:
      if( (millis() - timeStartCloseM2) >=  DELAYINMILLISEC/*-3000*/ ) {  // TODO: -3000 for debug: erase it quickly!
        returnState = CLOSE_M1;  // next state
      }
      else {
        returnState = CLOSE_DELAY;  // stay in delay
      }
    break;
    
    case CLOSE_M1:
      CloseGateM1();
      returnState = WAIT_CLOSE_END;
    break;
    
    case WAIT_CLOSE_END:
      if( (millis() - timeStartCloseM2) >=  TIMEOUTMOTORINMILLISEC ) {
        returnState = END_CLOSE;  // next state
      }
      else {
        if(isM1Stopped && isM2Stopped) {
          return END_CLOSE;
        }
        else {
          returnState = WAIT_CLOSE_END;  // stay in delay
        }
      }
    break;
    
    case END_CLOSE:
      StopGates();
      returnState = END_CLOSE;
    break;
    
    default:
      StopGates();  // stop when default
      returnState = END_CLOSE;
  }

  if( (millis()-lastTimeCheck) >= 1000 )
  {
    lastTimeCheck = millis();
    Serial.print("Closing time: "); Serial.print( (millis()-timeStartCloseM2)/1000 );  // time since the begining of the closing fsm
    Serial.print("; Current M1 = "); Serial.print(currentM1_inAmp); Serial.print("\tCurrent M2 = "); Serial.println(currentM2_inAmp);
  }
  
  return returnState;
}

void isort(int *a, int n)
{
  //Sorting function
  // sort function (Author: Bill Gentles, Nov. 12, 2010)
  //  *a is an array pointer function
  for (int i = 1; i < n; ++i) {
    int j = a[i]; int k;
    for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
      a[k + 1] = a[k];
    }
    a[k + 1] = j;
  }
}

int mode(int *x,int n){
  //Mode function, returning the mode or median.
  int i = 0;
  int count = 0;
  int maxCount = 0;
  int mode = 0;
  int bimodal;
  int prevCount = 0;
 
  while(prevCount&count>maxCount){
      mode=x[i];
      maxCount=count;
      bimodal=0;
    }
    if(count==0){
      i++;
    }
    if(count==maxCount){//If the dataset has 2 or more modes.
      bimodal=1;
    }
    if(mode==0||bimodal==1){//Return the median if there is no mode.
      mode=x[(n/2)];
    }
    return mode;
  }
  
void SetFlashLight() {
  timeStartFlashLight = millis();  // start flash light
  FlashLightState = true;
  digitalWrite(PIN_FLASHLIGHT, HIGH);
  SendMessage("Flashlight ON");
}  

void ResetFlashLight() {
  FlashLightState = false;
  digitalWrite(PIN_FLASHLIGHT, LOW);
  SendMessage("Flashlight OFF");
}

(It's not very easy to read because of the state machines.)

Discussions

satek.p wrote 03/20/2016 at 14:59 point

Hi Manu

could you implement multi button RC 

  Are you sure? yes | no

Manu wrote 03/14/2016 at 10:08 point

My project is essentially breadboard-based, so I don't have any schematic to post. I will try to find my old hand drawn schematic and scan it...

  Are you sure? yes | no

satek.p wrote 03/14/2016 at 08:55 point

Hi Manu

could you please share the schematic  

  Are you sure? yes | no

satek.p wrote 03/14/2016 at 08:55 point

Hi Manu

could you please share the schematic  

  Are you sure? yes | no

Manu wrote 09/06/2015 at 10:18 point

Hello, I tried with a humble opamp and it's not very satisfying. So, why not a dedicated current sensor chip. What type are you thinking about : INA210, ACS714, or something else?

Even though I haven't tried it yet, anything that looks like a differential amplifier would be better because when the second motor starts, the shunt resistors "see" a part of the current of the other motor. Not sure what is going on, but I may be good to improve that.

  Are you sure? yes | no

satek.p wrote 09/06/2015 at 01:54 point

Hi Manu

Its quite interesting !

Can we use current sensor chip to detect the peak and use it for limit as well as 

for protection ?

  Are you sure? yes | no