//M. Jouaneh Added operation by timer on 5/28/2019 // Changed to use H-bridge and force sensor 6/10/2019 //Version 10 Low Glue Value Changed to 550 //This comment was made on 12/10/19 to check if sketches are being saved properly #define DIR1 9 #define DIR2 8 #define PWMpin 10 void SetPosition(int); void DoControl(void); void GlueTask(void); //float startposition = 30; //float endposition = 100; //int x = startposition; int GlueState; int GlueNextState = 1; //Initialized to start at this state int Go2ReadyPosition = 0; int DispenseOn = 0; int glueReady = 0; //Signal to send to the Raspberry Pi when ready long LastTime; long LastTime2; int Enabled; // signal to send the actuator up or down float sumerr = 0.0; int Tsamp = 5; int advpos = 11; int retpos = 1; int prev = 0; int actpos2; int lastForceReading = 0; long GlueOnTime = 6000; //in ms long RetractDelay = 4000; float despos; int despos2; int ForceLevel = 10; //10 int Count; int lowGlueValue = 550; int glueLowVar = 0; bool actuatorUp = false; int posSum = 0; #define UpSwitch 4 #define DispSwitch 7 #define ReadySwitch 3 #define glueLow 11 #define PosChan 0 #define ForceChan 5 #define ready2Pi 2 void setup() { //ForceLevel = 205; //set pins pinMode(UpSwitch, INPUT); pinMode(DispSwitch, INPUT); pinMode(ReadySwitch, INPUT); pinMode(ready2Pi, OUTPUT); pinMode(glueLow, OUTPUT); pinMode(DIR1, OUTPUT); pinMode(DIR2, OUTPUT); pinMode(PWMpin, OUTPUT); //open serial window for log Serial.begin(38400); LastTime2 = millis(); Enabled = digitalRead(4); Go2ReadyPosition = 0; DispenseOn = 0; digitalWrite(glueLow, LOW); glueLowVar = 0; } void loop() { Serial.print("Enabled "); Serial.print(Enabled); Serial.print(" State "); Serial.print(GlueState); Serial.print(" Count "); Serial.print(Count); Serial.print(" DispenseOn "); Serial.print(DispenseOn); Serial.print(" Go2ReadyPos "); Serial.print(Go2ReadyPosition); Serial.print(" glueReady "); Serial.print(glueReady); Serial.print(" Glue Low "); Serial.print(glueLowVar); Serial.print(" Act pos "); Serial.print(actpos2); Serial.print(" Force "); Serial.println(lastForceReading); //Go2ReadyPosition = 1; //moved into setup GlueTask(); } void GlueTask(void) { Enabled = digitalRead(4); DispenseOn = digitalRead(7); Go2ReadyPosition = digitalRead(3); GlueState = GlueNextState; if ((Enabled == 0) && (actuatorUp == false)) { { digitalWrite(ready2Pi, LOW); SetPosition(1); //Move actuator to top position GlueNextState = 1; sumerr = 0; // reset error sum variable DoControl(); actpos2 = analogRead(PosChan); if (actpos2 < 100) { analogWrite(PWMpin, 0); actuatorUp = true; GlueNextState = 1; digitalWrite(glueLow, LOW); glueLowVar = 0; } } } else if (GlueState == 1) // Start state - actuator at 1% extension { if (Go2ReadyPosition == 1) { Count = 0; prev = 0; actuatorUp = false; GlueNextState = 2; } } else if (GlueState == 2) //Move actuator down { actpos2 = analogRead(PosChan); //read current position of actuator if (actpos2 > lowGlueValue) { digitalWrite(glueLow, HIGH); glueLowVar = 1; } if (actpos2 <= 950) { /////ADVANCE/////// digitalWrite(DIR1, LOW); digitalWrite(DIR2, HIGH); analogWrite(PWMpin, 200); //set the duty cycle or speed GlueNextState = 3; Count = 0; } } else if (GlueState == 3) //Check force sensor { actpos2 = analogRead(PosChan); //read position sensor lastForceReading = analogRead(ForceChan); if (actpos2 > lowGlueValue) { digitalWrite(glueLow, HIGH); glueLowVar = 1; } if (actpos2 >= 1010) // if actuator reached travel limit { //Enabled = 0; //Go2ReadyPosition = 0; digitalWrite(ready2Pi, LOW); GlueNextState = 1; } else if (lastForceReading >= ForceLevel) //if force reading above the threshold level { Count = Count + 1; prev = 1; GlueNextState = 4; } } else if (GlueState == 4) //wait for the proper # of counts { lastForceReading = analogRead(ForceChan); posSum = 0; for (int i = 1; i < 10; i++) { posSum = posSum + analogRead(PosChan); } actpos2 = posSum / 10; //analogRead(PosChan); if (actpos2 > lowGlueValue) { digitalWrite(glueLow, HIGH); Serial.print("GLUE LOW"); glueLowVar = 1; } if (actpos2 >= 1010) // if actuator reached travel limit { digitalWrite(ready2Pi, LOW); GlueNextState = 1; } else if (lastForceReading >= ForceLevel) { if (prev >= 1) { Count = Count + 1; } else { prev = 0; Count = 0; GlueNextState = 3; } if (Count >= 5) //play with count # { //Actuator is ready digitalWrite(ready2Pi, HIGH); //Go2ReadyPosition =0; //Reset the ready position command GlueNextState = 45; LastTime = millis(); if (actpos2 < (lowGlueValue-100)) { digitalWrite(glueLow, LOW); //After setup, this is the only way to return glueLow to LOW glueLowVar = 0; } digitalWrite(DIR1, HIGH); //reverse motion direction digitalWrite(DIR2, LOW); } } } else if (GlueState == 45) // Reverse motion { if (( millis() - LastTime) >= 150) { analogWrite(PWMpin, 0); //stop motion GlueNextState = 5; } } else if (GlueState == 5) //Waiting for new dispense command { if (DispenseOn == 1) { LastTime = millis(); digitalWrite(DIR1, LOW); digitalWrite(DIR2, HIGH); //set direction down analogWrite(PWMpin, 220); GlueNextState = 6; } } else if (GlueState == 6) // Reverse motion { if (( millis() - LastTime) >= GlueOnTime || (DispenseOn == 0)) { //Reverse motion direction // actpos2 = analogRead(PosChan); posSum = 0; for (int i = 1; i < 10; i++) { posSum = posSum + analogRead(PosChan); } actpos2 = posSum / 10; //analogRead(PosChan); if (actpos2 > lowGlueValue) { digitalWrite(glueLow, HIGH); glueLowVar = 1; } despos2 = actpos2 - 15; if (despos2 <= 10) { despos2 = 10; } digitalWrite(DIR1, HIGH); digitalWrite(DIR2, LOW); LastTime = millis(); GlueNextState = 7; } } else if (GlueState == 7)// Actuator Reached Retracted position { //actpos2 = analogRead(PosChan); //DoControl(); if ((millis() - LastTime) >= 150) //(actpos2 <= despos2) { //DispenseOn = 0; //Reset dispense command if not reset by user GlueNextState = 5; //Go to next ready position and wait for the next dispense command analogWrite(PWMpin, 0); } } } // end of GlueTask void SetPosition(int val) { // Val is a number between 0 - 100 despos = (val / 100.) * 1000.; } void DoControl(void) { int error; float kp = 10; float ki = 0.0; float output; int pwm; int current; int actpos; if ((millis() - LastTime2 ) >= Tsamp) { LastTime2 = millis(); actpos = analogRead(PosChan); current = analogRead(ForceChan); error = despos - actpos; // if ((abs(error)<10) && (moveComplete ==false)){ // moveComplete = true; // analogWrite(PWMpin,0); // } // else if ((moveComplete ==true) && (abs(error) >30)){ // moveComplete = false; // } output = kp * error + ki * sumerr; sumerr = error + sumerr; if (output > 0) { digitalWrite(DIR1, LOW); digitalWrite(DIR2, HIGH); } else { digitalWrite(DIR1, HIGH); digitalWrite(DIR2, LOW); } if (output >= 1023) output = 1023; if (output <= -1023) output = -1023; pwm = (abs(output) / 1023.0) * 255; analogWrite(PWMpin, pwm); } }