/*
BAND FEEDER
*/
/*
|USB|
LED Rewind | D13 D12 |>D (Motor Control B) to Motor Control
| 3.3V D11=|>D (Motor Control A) to Motor Control
| REF D10=| D LED Send
| A0/D14 D9=| D Switch LiftDown
LED LiftUp D | A1/D15 D8 | D Switch LiftUp
LiftDown D | A2/D16 D7 | D Switch Rewind
Sendor Exp D | A3/D17 D6 | D Switch Send
(i2c SDA DAC&LCD) <| A4 D5=| D PWM Servo
(i2c SCL DAC&LCD) <| A5 D4 | D Switch |Encoder
Sensor Pre A | A6 D3=| R Encode B
Sendor Poat A | A7 D2 | R Encode A
5V | 5V GND | 0V --------------------- 0v |-| to Motor Control +-- |-| to Motor Control
| RST RST | |-| to DAC | |-| to DAC-
GND | GND RX | |-| to Leveler | |-|
| VIN TX | |-| to LCD | |-| to LCD
|
[ A ] |
to MPU |-| GND from DC-DC |
to Panel | Switch LiftDown |-| GND | R - LED LiftDown |
| Switch LiftUp |-| GND | R - LED LiftUp |
| Switch Rewind |-| GND | R - LED Rewind |
| Switch Send |-| GND | R - LED Send |
| Encode Switch |-| GND to [C] |
to MPU |-| 5V from DC-DC| |
|-| 5V ----------------------------------------------------+
|-| 5V to [C]
[ B ]
|-|
|-|
to Pnael | LED Rewind |-| D13
| LED Send |=| D10
| Switch Liftdown |=| D9
| Switch LiftUp |-| D8
| Switch Rewind |-| D7
| Switch Send |-| D6
to Servo | PWM Servo |=| D5
to Encoder| Encode Switch |-| D4
| Encode B |=| D3
| Encode A |-| D2
[ C ]
|-| A0/D14
to Pnael | LED LiftUp |-| A1/D15
| LED LiftDown |-| A2/D16
to Sensor | Sensor Exp |-| A3/D17
| Sensor PreMotor |-| A6
| Sensor PostMotor |-| A7
| Gnd |-| GND from [A]
| 5V |-| 5V from [A]
*/
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MCP4725.h>
#include <Servo.h>
#include <MsTimer2.h>
#define P_OUT_MOTOR_A 11
#define P_OUT_MOTOR_B 12
#define P_PWM_SERVO 5
#define P_IN_FEED 6
#define P_IN_BACK 7
#define P_IN_LIFTUP 8
#define P_IN_LIFTDOWN 9
#define P_IN_ENCODE_A 2
#define P_IN_ENCODE_B 3
#define P_IN_ENCODE_SWITCH 4
#define P_IN_PREMOTOR A6
#define P_IN_POSTMOTOR A7
#define P_IN_EXPAND A3 // no use
LiquidCrystal_I2C lcd(0x27, 16 , 2); //3F
MCP4725 dac(0x61);
#define P_LED_FEED 10
#define P_LED_BACK 13
#define P_LED_LIFTUP 15
#define P_LED_LIFTDOWN 16
#define SW_FEED 0x01
#define SW_BACK 0x02
#define SW_LIFTUP 0x04
#define SW_LIFTDOWN 0x08
#define SNS_PRE 0x0100 // PreMotor
#define SNS_POST 0x0200 // PostMotor
#define SNS_LIFTLOW 0x0400 // Lift Low
#define SW_SETUP 0x1000 // Switch SETUP
char *message1 = "Band Feeder ";
char *message2 = "20240131 ";
char* msgClear = " ";
// ======================================================================================================
// INITIAL PARAMETER
typedef struct {
char* msg;
int min;
int max;
} controlParam;
#define SETUPNUMBER 7
#define SETUP_INITVOLT 0
#define SETUP_INITTIME 1
#define SETUP_FEEDVOLT 2
#define SETUP_FEEDTIME 3
#define SETUP_BACKVOLT 4
#define SETUP_LIFTUP 5
#define SETUP_LIFTDOWN 6
controlParam param[SETUPNUMBER] = {
{"InitVolt : ", 1, 50},
{"InitTime : ", 1, 50},
{"FeedVolt : ", 1, 50},
{"FeedTime : ", 1, 99},
{"BackVolt : ", 1, 50},
{"LiftLow : ", 0, 99},
{"LiftHi : ", 0, 99}
};
int paramVol[SETUPNUMBER] = {5,40,20,99,15,30,80};
#define paramLiftHi paramVol[SETUP_LIFTUP]
#define paramLiftLow paramVol[SETUP_LIFTDOWN]
#define paramFeedVolt paramVol[SETUP_FEEDVOLT]
#define paramBackVolt paramVol[SETUP_BACKVOLT]
#define paramInitVolt paramVol[SETUP_INITVOLT]
#define paramInitTime paramVol[SETUP_INITTIME]
#define paramFeedTime paramVol[SETUP_FEEDTIME]
int locateSetup; // param[ N ]
int modeSetup;
unsigned long setupEntryTime = 5000L; // keep Push Encoder Switch 10sec
unsigned long setupEntryStartTime;
void startEntryTimer() {
setupEntryStartTime = millis();
}
unsigned long elapsedTime() {
return millis() - setupEntryStartTime;
}
int getParamVal() {
return paramVol[ locateSetup ];
}
// ======================================================================================================
// LDC DISPLAY
char* strVol(int n) {
static char code[10];
n %= 100;
code[0] = (n/10)+'0';
code[1] = '.';
code[2] = (n%10)+'0';
code[3] = 0;
return &code[0];
}
char* strParamNum(int loc) {
static char code[10];
return strVol(paramVol[loc]);
}
void dispParam(int loc) {
if (loc < 0 ) {
loc = 0;
} else if (loc >(SETUPNUMBER-1)) {
loc = SETUPNUMBER-1;
}
// 0123456789012345
lcd.setCursor(0,0); lcd.print("SETUP /6");
lcd.setCursor(13,0); lcd.print(loc+1);
lcd.setCursor(0,1); lcd.print(param[loc].msg);
lcd.setCursor(13,1); lcd.print(strParamNum(loc));
Serial.print("- ");
Serial.print(locateSetup);
Serial.print(" : ");
Serial.print(paramVol[loc]);
Serial.print(" - ");
Serial.println(strParamNum(loc));
}
void dispParamVol() {
lcd.setCursor(13,1); lcd.print(strParamNum(locateSetup));
}
void dispCurParam() {
dispParam(locateSetup);
}
int incParam() {
if(++locateSetup >= 6)
locateSetup = 0;
return locateSetup;
}
// ======================================================================================================
// SWITCH
/*
---- ---- ---- xxxx
| ||| ||||___ SW_FEED
| ||| |||____ SW_BACK
| ||| ||_____ SW_LIFTUP
| ||| |______ SW_LIFTDOWN
| |||
| |||_____________ SNS_PRE
| ||______________ SNS_POST
| |_______________ SNS_EXP
|
|__________________ SW_SETUP
*/
int switchCode;
void initSwitch() {
// Switch
pinMode(P_IN_FEED, INPUT_PULLUP);
pinMode(P_IN_BACK, INPUT_PULLUP);
pinMode(P_IN_LIFTUP, INPUT_PULLUP);
pinMode(P_IN_LIFTDOWN, INPUT_PULLUP);
pinMode(P_IN_ENCODE_SWITCH, INPUT_PULLUP);
}
unsigned int readSwitch() {
static int sw;
sw = 0;
sw |= digitalRead(P_IN_FEED)==0 ? SW_FEED : 0; // Switch Send Band
sw |= digitalRead(P_IN_BACK)==0 ? SW_BACK : 0; // Switch Rewind Band
sw |= digitalRead(P_IN_LIFTUP)==0 ? SW_LIFTUP : 0; // Switch Liftup Arm
sw |= digitalRead(P_IN_LIFTDOWN)==0 ? SW_LIFTDOWN : 0; // Switch Liftdown Arm
sw |= digitalRead(P_IN_ENCODE_SWITCH)==0 ? SW_SETUP : 0; // Switch Setup
sw |= analogRead(P_IN_PREMOTOR)<100 ? SNS_PRE : 0; // sensor Pre Motor
sw |= analogRead(P_IN_POSTMOTOR)<100 ? SNS_POST : 0; // sensor Post Motor
// sw |= analogRead(P_IN_EXPAND)<500 ? SNS_EXP : 0; // sensor Expand (no use)
return sw;
}
unsigned int _swc[8];
//unsigned int __swc;
unsigned int denoiseSwitch(unsigned int sw, int lengh) {
// static unsigned int _sw[6];
_swc[5] = _swc[4];
_swc[4] = _swc[3];
_swc[3] = _swc[2];
_swc[2] = _swc[1];
_swc[1] = _swc[0];
_swc[0] = sw;
// Serial.println();
static unsigned int _dsw;
_dsw = 0;
for(int i=0; i<lengh; i++) {
_dsw |= _swc[i];
}
return _dsw;
}
// ======================================================================================================
// LIFT SERVO
#define LIFT_LOW 0
#define LIFT_HI 1
Servo servo; // create Class
int servoPos;
/*
int servoLowPos = 50;
int servoHiPos = 70;
*/
void initServo() {
servo.attach(5);
servo.write(paramLiftLow);
}
// pos 0 : Low / 1 : High
void moveServo(int pos) {
servoPos = pos; // LOW / HIGH
servo.write( pos==LIFT_LOW ? paramLiftLow : paramLiftHi );
}
// ======================================================================================================
// LOAD SENSOR
int bandStatus;
#define BAND_NO_LOAD 0
#define BAND_FRONT 1
#define BAND_LOADED 2
#define BAND_END 3
#define BAND_ERROR -1
char* msgBandStat[5] = {
// 0123456789012345
"ERROR ",
"BAND NO LOAD ",
"BAND PRE MOTOR ",
"BAND LOADED ",
"BAND END "
};
//unsigned int bandSens;
int statSensor(unsigned int _sw) {
_sw &= (SNS_PRE | SNS_POST);
int bandStat;
// bandSens = _sw;
switch(_sw) {
case 0: bandStat = BAND_NO_LOAD; break;
case SNS_PRE: bandStat = BAND_FRONT; break;
case SNS_POST: bandStat = BAND_END; break;
case SNS_PRE|SNS_POST: bandStat = BAND_LOADED; break;
defaut: bandStat = BAND_ERROR; break;
}
return bandStatus = bandStat;
}
void dispBandStat() {
lcd.setCursor(0,0); lcd.print(msgBandStat[bandStatus+1]);
}
int sensorStat() {
return bandStatus;
}
// ======================================================================================================
// MOTOR
#define MOTOR_FEED_LOW 1
#define MOTOR_FEED_HI 2
#define MOTOR_BACK_LOW -1
#define MOTOR_BACK_HI -2
#define MOTOR_STOP 0
float AdjastSpeedVolt = 0.2;
int motorStatus; // 0:STOP 1:RUN
unsigned long motorOprationTime;
int motorVector;
int startVoltage;
int feedVoltage;
int backVoltage;
//int startTime;
int feedTime;
int accelationTime; // 500msec ateptime:50msec incliment:10
int currentVoltage;
int targetVoltage;
unsigned long startTime;
unsigned long currentTime; // msec
unsigned long targetTime; // msec
int currentVect;
void vectMotor(int vect) {
switch(currentVect = vect) {
case 2: digitalWrite(P_OUT_MOTOR_A,1); digitalWrite(P_OUT_MOTOR_B,0);
targetTime = paramFeedTime * 100L;
currentVoltage = paramInitVolt; targetVoltage = paramFeedVolt;
break;
case 1: digitalWrite(P_OUT_MOTOR_A,1); digitalWrite(P_OUT_MOTOR_B,0);
targetTime = paramInitTime * 100L; startTime = millis();
currentVoltage = targetVoltage = paramInitVolt;
Serial.println("FEED");
break;
case -1: digitalWrite(P_OUT_MOTOR_A,0); digitalWrite(P_OUT_MOTOR_B,1);
targetTime = 0;
currentVoltage = targetVoltage = paramInitVolt;
break;
case -2: digitalWrite(P_OUT_MOTOR_A,0); digitalWrite(P_OUT_MOTOR_B,1);
targetTime = 0;
currentVoltage = targetVoltage = paramBackVolt;
break;
case -3: digitalWrite(P_OUT_MOTOR_A,1); digitalWrite(P_OUT_MOTOR_B,0);
targetTime = 500L; startTime = millis();
currentVoltage = targetVoltage = paramInitVolt;
break;
default: digitalWrite(P_OUT_MOTOR_A,0); digitalWrite(P_OUT_MOTOR_B,0);
targetTime = 0;
currentVoltage = targetVoltage = 0L;
currentVect = 0;
break;
}
}
void driveMotor() {
dac.setVoltage((float)currentVoltage/10.0);
if (targetVoltage > currentVoltage) {
currentVoltage++;
}
switch(currentVect) {
case 1: if (millis() > (startTime + targetTime)) {
vectMotor(2);
Serial.println("1 > 2");
}
break;
case 2: if (millis() > (startTime + targetTime)) {
vectMotor(0);
Serial.println("2 > 0");
} else {
if(currentVoltage < targetVoltage) currentVoltage++;
delay(100);
}
break;
case -1:
case -2: if ( (switchCode & SNS_POST) == 0) {
vectMotor(0);
}
break;
case -3: if (millis() > (startTime + targetTime)) {
vectMotor(0);
Serial.println("-3 > 0");
}
break;
default:
startTime = millis();
break;
}
}
// ======================================================================================================
// ENCODER
int volChenged;
void initEncoder() {
// Serial.println("ENCODER");
pinMode(P_IN_ENCODE_A, INPUT_PULLUP);
pinMode(P_IN_ENCODE_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(P_IN_ENCODE_A),pluse_counter,FALLING);
}
int countEncode = 0;
void pluse_counter() {
int n = paramVol[locateSetup];
if(digitalRead(P_IN_ENCODE_A)^digitalRead(P_IN_ENCODE_B)) {
n++;
} else {
n--;
}
volChenged = 1;
if ( n > param[locateSetup].max) n = param[locateSetup].min;
if ( n < param[locateSetup].min) n = param[locateSetup].max;
paramVol[locateSetup] = n;
//Serial.print(n);
}
// ======================================================================================================
// SETUP
void setup() {
Serial.begin(19200);
delay(200);
lcd.init();
lcd.backlight();
delay(500);
Serial.println();
Serial.println(message1);
Serial.println(message2);
Serial.println();
lcd.setCursor(0,0); lcd.print(message1);
lcd.setCursor(0,1); lcd.print(message2);
dac.begin();
dac.setMaxVoltage(5.0);
delay(100);
pinMode(P_LED_FEED,OUTPUT);
pinMode(P_LED_BACK,OUTPUT);
pinMode(P_LED_LIFTUP,OUTPUT);
pinMode(P_LED_LIFTDOWN,OUTPUT);
pinMode(P_OUT_MOTOR_A,OUTPUT);
pinMode(P_OUT_MOTOR_B,OUTPUT);
modeSetup = 0;
initSwitch();
initEncoder();
initServo();
}
int lastVect;
unsigned int lastSw;
unsigned int last_Sw;
int lastVol;
void loop() {
unsigned int sw;
unsigned int _sw;
// ------------------------------------------------------------------
// read SWITCH
sw = readSwitch();
switchCode = sw;
// Serial.print(sw,BIN);
// Serial.print(" : ");
sw = denoiseSwitch(sw, 4);
// Serial.print(sw,BIN);
// Serial.print(" ");
// switchCode = sw;
// ------------------------------------------------------------------
// set BAND SENSOR STATUS
statSensor(sw);
// ------------------------------------------------------------------
// function SETUP FEED BACK LIFTUP LIFTDOWN
_sw = sw & (SW_SETUP|SW_FEED|SW_BACK|SW_LIFTUP|SW_LIFTDOWN);
if(last_Sw != _sw) {
if (modeSetup == 1) {
if(sw & (SW_FEED|SW_BACK)!=0) {
modeSetup = 0;
lcd.setCursor(0,0); lcd.print(message1);
}
}
//
switch(_sw) {
// ------------------------------------------------
case SW_SETUP:
{
if(modeSetup == 0) {
startEntryTimer();
} else {
incParam();
dispCurParam();
}
break;
}
// ------------------------------------------------
case SW_FEED:
{
if (modeSetup) break;
if (servoPos != LIFT_LOW) break;
if (bandStatus != BAND_LOADED) break;
vectMotor(1);
lcd.setCursor(0,1); lcd.print("<< SEND ");
break;
}
// ------------------------------------------------
case SW_BACK:
{
if (bandStatus != BAND_LOADED) break;
if (servoPos == LIFT_LOW) {
vectMotor(-1);
} else {
vectMotor(-2);
}
break;
}
// ------------------------------------------------
case SW_LIFTUP:
{
moveServo(LIFT_HI);
break;
}
// ------------------------------------------------
case SW_LIFTDOWN:
{
moveServo(LIFT_LOW);
break;
}
}
} else {
// Switch continue
switch(_sw) {
// ------------------------------------------------
case SW_SETUP:
if (modeSetup == 0) {
if (elapsedTime() >= setupEntryTime) {
modeSetup = 1;
dispParam(locateSetup = 0);
}
}
break;
// ------------------------------------------------
case SW_FEED:
driveMotor();
break;
// ------------------------------------------------
case SW_BACK:
driveMotor();
break;
// ------------------------------------------------
default:
vectMotor(0);
}
}
switch(currentVect) {
case -1:
case -2:
if ((switchCode & SNS_POST) == 0) {
}
break;
}
if (modeSetup) {
if(volChenged) {
dispParamVol();
volChenged = 0;
switch(locateSetup) {
case SETUP_LIFTUP:
moveServo(LIFT_HI);
break;
case SETUP_LIFTDOWN:
moveServo(LIFT_LOW);
break;
}
}
} else {
int _val = getParamVal();
dispBandStat();
if (_val != lastVol) {
dispParamVol();
}
lastVol = _val;
}
// ------------------------------------------------------------------
// LCD 2nd Line
if (currentVect != lastVect) {
switch(currentVect) {
case 0: lcd.setCursor(0,1); lcd.print(msgClear); break;
case 1:
case 2: lcd.setCursor(0,1); lcd.print("<< SEND "); break;
case -1:
case -2: lcd.setCursor(0,1); lcd.print("<< BACK "); break;
}
} else {
switch(currentVect) {
case 1:
case 2:
case -1:
case -2:
case -3: lcd.setCursor(13,1); lcd.print(strVol(currentVoltage)); break;
}
}
// ------------------------------------------------------------------
// L E D
switch(servoPos) {
case LIFT_HI: digitalWrite(P_LED_LIFTUP,1); break;
case LIFT_LOW: digitalWrite(P_LED_LIFTDOWN,1); break;
default: digitalWrite(P_LED_LIFTUP,0); digitalWrite(P_LED_LIFTDOWN,0); break;
}
// digitalWrite(P_LED_FEED,(sw & SW_FEED));
// digitalWrite(P_LED_BACK,(sw & SW_BACK));
digitalWrite(P_LED_LIFTUP,(sw & SW_LIFTUP));
digitalWrite(P_LED_LIFTDOWN,(sw & SW_LIFTDOWN));
lastSw = sw;
last_Sw = _sw;
lastVect = currentVect;
delay(50);
}