Peter Betz
DUAL inputs code
Dependencies: mbed reScale USBDevice
main.cpp
- Committer:
- BETZtechnik
- Date:
- 2019-10-06
- Revision:
- 25:baedb9e32b6d
- Parent:
- 10:a3ada529b264
File content as of revision 25:baedb9e32b6d:
#include "mbed.h"
#include "reScale.h"
// 2019-10-01: Updated to include pulse, Nozzle direction.
//Serial pc(USBTX, USBRX);
//InterruptIn aux1(P0_7, PullUp); // index pulse
DigitalIn aux1(P0_7);
DigitalIn aux2(P1_21);
DigitalIn cw(P1_24);
DigitalIn ccw(P2_7);
DigitalIn pulse(P0_6);
int cwRequested = 0;
int ccwRequested = 0;
int pulseActive = 0;
int pulseFlag =1; // is pulse high or low. (off is high after the opto)
int pulseStart = 0;
int pulseCount = 0;
int lastPulse = 0; // for debounce
int pulseDebounce = 1; //ms for debounce pulse signal
int aux1State = 0;
int autoMode = 0; // setting for AUTO mode (X7a, Pulse etc.)
int aux2Mode =0; // aux 2 use, twitch wait, etc.
int twitchWaitFlag = 0; // was twitch wait enabled?
int waitFlag = 0;
DigitalOut dir(P0_15);
DigitalOut auxOut1(P0_8);
Serial display(P0_14, P0_13, 19200); // RS 485 TX RX?
Serial inputs(P1_23,P0_20, 9600); //rs232 TX RX
int maxServo = 2500;
int minServo = 500;
reScale servo1Scale(90,-90,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
PwmOut servo1(P1_13); // PRODUCTION
//PwmOut servo1(P2_2); // TEST!!!
//char tens = '0';
//char ones = '1';
int Hundreds = 0;
int Tens = 0;
int Ones = 0;
int twitch = 0;
int twitchStart=0;
int twitchDirection=0;
int lastTwitchMove = 0;
int maxTwitch1 = 0;
int maxTwitch2 = 0;
int minTwitch1 = 0;
int minTwitch2 = 0;
int tReadMs = 5;
int twitchIncrement = 10;
int currentNozzle = 0;
int index = 0; // for parsing serial
int n1Pos = 0;
int lastN1Called = 0; // store the last actual tool position sent from the display because twitch code can shift it.
int n1Dir = 0;
int servo1Pos = 0;
int lastServo1Pos = 0;
//DigitalOut servo2(P2_18);
/*
void triggered()
{
servo2 = 1;
if (cw == 0){
cwRequested = 1;
}
else if (ccw == 0){
ccwRequested = 1;
}
}
*/
Timer t;
int main()
{
servo1.period(0.020); // servo requires a 20ms period
cw.mode(PullUp);
ccw.mode(PullUp);
aux1.mode(PullUp);
aux2.mode(PullUp);
pulse.mode(PullUp);
// auxOut1 = 1;
t.start();
lastPulse = t.read_ms();
//aux1.rise(&triggered);
//servo2 = 0;
uint8_t c = 0; // for 485 link
while(1) {
if (t.read() > 1200) {
t.reset();
lastTwitchMove = t.read_ms();
lastPulse = t.read_ms();
}
//************************************ Interrupt response **********************
if (aux1 == 0 && aux1State == 0) {
wait(0.1); //**************************2019-06-27
if (cw == 1) {
//servo2 = 1;
dir= 1;
//wait(0.001);
display.printf("M");
wait(0.05);
display.printf("-");
wait(0.001);
dir=0;
aux1State = 1;
//wait(.1);
}
if (ccw == 1) {
dir= 1;
//wait(0.01);
display.printf("M");
wait(0.05);
display.printf("+");
wait(0.001);
dir=0;
aux1State = 1;
//wait(.1);
}
}
if (aux1 == 1 && aux1State == 1) {
aux1State = 0;
//wait(0.1);
}
//********************** Tool Pulse **********************************
/*
if (pulse == 0)
{
pulseCount = 103;
dir=1;
display.printf("T");
wait(0.05);
display.printf("%d", pulseCount);
wait(0.001);
dir=0;
wait(0.2);
}
*/
/*
if (pulse == 0 && pulseActive == 0)
{
pulseStart = t.read_ms();
pulseActive = 1;
pulseCount = 1;
wait(0.01);
}
if (pulse == 0 && pulseActive == 1 && (t.read_ms() < pulseStart + 1000))
{
pulseCount = pulseCount + 1;
wait(0.01);
}
if (pulseActive == 1 && (t.read_ms() > pulseStart + 1000))
{
pulseCount = pulseCount + 100;
dir=1;
display.printf("T");
wait(0.05);
display.printf("%d", pulseCount);
wait(0.001);
dir=0;
pulseCount = 0;
pulseActive = 0;
wait(0.01);
}
*/
if (pulse == 0 && pulseActive == 0 && pulseFlag == 1 && (t.read_ms() > (lastPulse + pulseDebounce))) {
pulseStart = t.read_ms();
lastPulse = t.read_ms();
pulseActive = 1;
pulseCount = 1;
pulseFlag = 0; // pin is now low
//wait(0.01);
}
if (pulse == 1 && pulseActive == 1 && pulseFlag == 0 && (t.read_ms() > lastPulse + pulseDebounce)) {
pulseFlag = 1;
lastPulse = t.read_ms();
}
if (pulse == 0 && pulseActive == 1 && pulseFlag == 1 && (t.read_ms() > lastPulse + pulseDebounce)) {
pulseCount = pulseCount + 1;
pulseFlag =0;
lastPulse = t.read_ms();
//wait(0.01);
}
if (pulseActive == 1 && (t.read_ms() > pulseStart + 1000)) {
pulseCount = pulseCount + 100;
dir=1;
display.printf("T");
wait(0.05);
display.printf("%d", pulseCount);
wait(0.001);
dir=0;
pulseCount = 0;
pulseActive = 0;
pulseFlag=1;
lastPulse = t.read_ms();
//wait(0.01);
}
//********************** Display code NEW with W **********************************
while (display.readable()) {
c = display.getc();
if (c == 'N') {
index = 1;
}
if (c == 'W') { // wiggle setting
index = 5;
}
if (c == 'A') { // auto mode
index = 8;
}
if (c == 'I') { // Input mode
index = 9;
}
if (c == 'D') { // nozzle direction
c = display.getc();
if (c == '1') { // N1
index = 6;
}
}
/*
if (c == '2'){ // N2
index = 7;
}
}
*/
if (index == 5) {
c = display.getc();
if (c == '0') {
twitch = 0;
index = 0;
}
if (c == '1') {
twitch = 1;
index = 0;
}
if (c == '2') {
twitch = 2;
index = 0;
}
if (c == '3') {
twitch = 3;
index = 0;
}
}
if (index == 6) {
c = display.getc();
if (c == '0') {
n1Dir = 0;
index = 0;
}
if (c == '1') {
n1Dir = 1;
index = 0;
}
}
if (index == 8) { // auto mode
c = display.getc();
if (c == '1') {
autoMode = 1; // X7a (x7 atc)
index = 0;
}
if (c == '2') {
autoMode = 2; // Pulse
index = 0;
}
}
if (index == 9) { // aux2 mode
c = display.getc();
if (c == '0') {
aux2Mode = 0; // 0= off, 1 = twitch pause wait for coolant signal
index = 0;
}
if (c == '1') {
aux2Mode = 1; // twitch pause
index = 0;
}
}
if (index == 1) { //Display is sending -90>+90 and adding 190 for N1 so would send 100 to 280 for -90 to +90
c = display.getc();
if (c == '1') { // N1
currentNozzle = 1;
Hundreds = 0;
index = 3;
}
else if (c == '2') { //N2
currentNozzle = 1;
Hundreds = 1;
index = 3;
}
if (c == '3') { //N2 // Display adding 390 for N2 so 300 to 480 would be -90 to +90
currentNozzle = 2;
Hundreds = 0;
index = 3;
}
if (c == '4') { //N2
currentNozzle = 2;
Hundreds = 1;
index = 3;
}
}
if (index == 3) {
c = display.getc();
if (c=='0') {
Tens = 0;
index = 4;
}
else if (c=='1') {
Tens = 1;
index = 4;
}
else if (c=='2') {
Tens = 2;
index = 4;
}
else if (c=='3') {
Tens = 3;
index = 4;
}
else if (c=='4') {
Tens = 4;
index = 4;
}
else if (c=='5') {
Tens = 5;
index = 4;
}
else if (c=='6') {
Tens = 6;
index = 4;
}
else if (c=='7') {
Tens = 7;
index = 4;
}
else if (c=='8') {
Tens = 8;
index = 4;
}
else if (c=='9') {
Tens = 9;
index = 4;
}
}
if (index == 4) {
c = display.getc();
if (c=='0') {
Ones = 0;
}
else if (c=='1') {
Ones = 1;
}
else if (c=='2') {
Ones = 2;
}
else if (c=='3') {
Ones = 3;
}
else if (c=='4') {
Ones = 4;
}
else if (c=='5') {
Ones = 5;
}
else if (c=='6') {
Ones = 6;
}
else if (c=='7') {
Ones = 7;
}
else if (c=='8') {
Ones = 8;
}
else if (c=='9') {
Ones = 9;
}
if (currentNozzle == 1) {
n1Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
twitchStart = 0; // trigger sweep from new angle
servo1Pos = servo1Scale.from(n1Pos);
lastN1Called = servo1Pos;
//servo1.pulsewidth_us(servo1Pos);
index=0;
}
}
}
/*
if (twitch == 0 || (twitch > 0 && aux2Mode == 1 && aux2 == 0)) {
if (servo1Pos != lastServo1Pos) {
twitchStart = 0;
twitchDirection = 0;
servo1Pos = servo1Scale.from(n1Pos);
servo1.pulsewidth_us(servo1Pos);
//servo2Pos = servo2Scale.from(n2Pos);
//servo2.pulsewidth_us(servo2Pos);
lastServo1Pos = servo1Pos;
if (twitch > 0 && aux2Mode == 1 && aux2 == 0 && last
}
}
if (twitch > 0) {
if (twitchStart == 0) {
//tw.start(); // start timer for nozzle sweep
//servo1Pos = servo1Scale.from(n1Pos);
servo1.pulsewidth_us(servo1Pos);
lastTwitchMove = t.read_ms(); // store time of last nozzle movement
minTwitch = servo1Pos; // store original servo postion for bottom of sweep
if (twitch == 1) {
maxTwitch = servo1Pos - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
}
if (twitch == 2) {
maxTwitch = servo1Pos - 150;
}
if (twitch == 3) {
maxTwitch = servo1Pos - 200;
}
twitchStart = 1;
}
if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
if (twitchDirection == 0) { //going up
servo1Pos = servo1Pos - 10; // add variable amount of uS to the servo signal
if (servo1Pos < minServo) {
servo1Pos = minServo;
twitchDirection = 1; //reverse direction
}
if (servo1Pos < maxTwitch) {
servo1Pos = maxTwitch;
twitchDirection = 1; //reverse direction
}
lastTwitchMove = t.read_ms();
}
if (twitchDirection == 1) { // going down
servo1Pos = servo1Pos + 10; // add variable amount of Ms to the servo signal
if (servo1Pos > maxServo) {
servo1Pos = maxServo;
twitchDirection = 0; //reverse direction
}
if (servo1Pos > minTwitch) {
servo1Pos = minTwitch;
twitchDirection = 0; //reverse direction
}
lastTwitchMove = t.read_ms();
}
if (servo1Pos != lastServo1Pos) {
servo1.pulsewidth_us(servo1Pos);
lastServo1Pos = servo1Pos;
}
}
}
*/
if ((twitch == 0) || (twitch > 0 && aux2Mode == 1 && aux2 == 1)) {
if (servo1Pos != lastServo1Pos) {
twitchStart = 0;
twitchDirection = 0;
servo1Pos = servo1Scale.from(n1Pos);
servo1.pulsewidth_us(servo1Pos);
lastServo1Pos = servo1Pos;
}
}
/*
dir=1;
display.printf("X");
// wait(0.05);
// display.printf("1");
wait(0.05);
dir=0;
waitFlag = 1;
wait(0.5);
*/
if (twitch > 0 && aux2Mode != 1 || (twitch > 0 && aux2Mode == 1 && aux2 == 0)) {
if (twitchStart == 0) {
//tw.start(); // start timer for nozzle sweep
//servo1Pos = servo1Scale.from(n1Pos);
servo1.pulsewidth_us(servo1Pos);
lastTwitchMove = t.read_ms(); // store time of last nozzle movement
minTwitch1 = lastN1Called; // store original servo postion for bottom of sweep
if(n1Dir == 0) {
if (twitch == 1) {
maxTwitch1 = lastN1Called - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
}
if (twitch == 2) {
maxTwitch1 = lastN1Called - 150;
}
if (twitch == 3) {
maxTwitch1 = lastN1Called - 200;
}
}
if(n1Dir == 1) { // reverse
if (twitch == 1) {
maxTwitch1 = lastN1Called + 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
}
if (twitch == 2) {
maxTwitch1 = lastN1Called + 150;
}
if (twitch == 3) {
maxTwitch1 = lastN1Called + 200;
}
}
twitchStart = 1;
}
if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
if (twitchDirection == 0) { //going up
if (n1Dir ==0) {
servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
}
if (n1Dir ==1) {
servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
}
if (n1Dir ==0 && servo1Pos < maxTwitch1 ) {
servo1Pos = maxTwitch1;
twitchDirection = 1; //reverse direction
}
// if (n2Dir == 0 && servo2Pos < maxTwitch2) {
// servo2Pos = maxTwitch2;
// twitchDirection = 1; //reverse direction
// }
if (n1Dir ==1 && servo1Pos > maxTwitch1) {
servo1Pos = maxTwitch1;
twitchDirection = 1; //reverse direction
}
// if (n2Dir == 1 && servo2Pos > maxTwitch2) {
// servo2Pos = maxTwitch2;
// twitchDirection = 1; //reverse direction
// }
lastTwitchMove = t.read_ms();
}
if (twitchDirection == 1) { // going down
if (n1Dir ==0) {
servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
}
if (n1Dir ==1) {
servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
}
/*
if (n1Dir == 0 && servo1Pos < minServo) {
servo1Pos = minServo;
twitchDirection = 0; //reverse direction
}
*/
if (n1Dir == 0 && servo1Pos > (minTwitch1+50)) {
servo1Pos = minTwitch1;
twitchDirection = 0; //reverse direction
}
if (n1Dir == 1 && servo1Pos < (minTwitch1 - 50)) {
servo1Pos = minTwitch1;
twitchDirection = 0; //reverse direction
}
// if (n2Dir == 0 && servo2Pos > (minTwitch2+0)) {
// servo2Pos = minTwitch2;
// twitchDirection = 0; //reverse direction
// }
// if (n2Dir == 1 && servo2Pos < minTwitch2) {
// servo2Pos = minTwitch2;
// twitchDirection = 0; //reverse direction
// }
lastTwitchMove = t.read_ms();
}
if (servo1Pos != lastServo1Pos) {
servo1.pulsewidth_us(servo1Pos);
lastServo1Pos = servo1Pos;
}
}
}
if (twitch >0 && aux2Mode == 1 && aux2 == 1) {
if (waitFlag == 0) {
dir=1;
display.printf("X");
wait(0.05);
display.printf("X");
wait(0.001);
dir=0;
waitFlag =1;
// wait (0.1);
}
}
if ((twitch == 0) || (twitch >0 && aux2Mode == 1 && aux2 == 0)|| (twitch >0 && aux2Mode == 0)) {
if (waitFlag == 1) {
dir=1;
display.printf("Y");
wait(0.05);
display.printf("Y");
wait(0.001);
dir=0;
waitFlag =0;
// wait (0.1);
}
}
//********************************* Inputs serial *******************************
while (inputs.readable()) {
c = inputs.getc();
if (c == 'T') {
index = 1;
//servo2=1;
}
if (index == 1) {
c = inputs.getc();
if (c=='0') {
Tens = 0;
//servo2=1;
}
else if (c=='1') {
Tens = 1;
}
else if (c=='2') {
Tens = 2;
}
else if (c=='3') {
Tens = 3;
}
else if (c=='4') {
Tens = 4;
}
else if (c=='5') {
Tens = 5;
}
else if (c=='6') {
Tens = 6;
}
else if (c=='7') {
Tens = 7;
}
else if (c=='8') {
Tens = 8;
}
else if (c=='9') {
Tens = 9;
}
else if (c=='0') {
Tens = 0;
}
index = 2;
}
if (index == 2) {
c = inputs.getc();
if (c=='0') {
Ones = 0;
}
else if (c=='1') {
Ones = 1;
}
else if (c=='2') {
Ones = 2;
}
else if (c=='3') {
Ones = 3;
}
else if (c=='4') {
Ones = 4;
}
else if (c=='5') {
Ones = 5;
}
else if (c=='6') {
Ones = 6;
}
else if (c=='7') {
Ones = 7;
}
else if (c=='8') {
Ones = 8;
}
else if (c=='9') {
Ones = 9;
}
else if (c=='0') {
Ones = 0;
}
int tempToolNo = (Tens*10)+100+Ones;
dir=1;
display.printf("T");
wait(0.05);
display.printf("%d", tempToolNo);
wait(0.001);
dir=0;
index = 0;
tempToolNo=0;
}
}
// *****************************************************************************
}
}