Mark and Peter
Remote inc coolant nozzle control program
Dependencies: mbed reScale USBDevice
Dependents: Nozzle_inputs Nozzle_display
Diff: main.cpp
- Revision:
- 24:55fafbb82a37
- Parent:
- 22:a3ada529b264
- Child:
- 25:5c71b4d95638
--- a/main.cpp Thu Apr 11 23:22:45 2019 +0000
+++ b/main.cpp Sun Sep 15 18:40:26 2019 +0000
@@ -2,6 +2,7 @@
#include "reScale.h"
+// DUAL NOZZLE VERSION 1.2/ 1.3 only!!
//Serial pc(USBTX, USBRX);
@@ -10,14 +11,24 @@
DigitalIn aux1(P0_7);
DigitalIn cw(P1_24);
DigitalIn ccw(P2_7);
+DigitalIn pulse(P1_21);
+//InterruptIn pulse(P1_21);
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;
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
@@ -26,8 +37,11 @@
int maxServo = 2500;
int minServo = 500;
reScale servo1Scale(90,-90,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
+reScale servo2Scale(90,-90,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
- PwmOut servo1(P1_13); //display board
+
+ PwmOut servo1(P2_2); //display board
+ PwmOut servo2(P2_19);
//char tens = '0';
//char ones = '1';
@@ -40,21 +54,34 @@
int twitchStart=0;
int twitchDirection=0;
int lastTwitchMove = 0;
- int maxTwitch = 0;
- int minTwitch = 0;
- int tReadMs = 10;
+ 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 n2Pos = 0;
+
+int lastN1Called = 0; // store the last actual tool position sent from the display because twitch code can shift it.
+int lastN2Called = 0;
+
+int n1Dir = 0;
+int n2Dir = 0;
int servo1Pos = 0;
int lastServo1Pos = 999;
+
+ int servo2Pos = 0;
+ int lastServo2Pos = 999;
-DigitalOut servo2(P2_2);
+//DigitalOut servo2(P2_2);
/*
void triggered()
@@ -75,20 +102,36 @@
Timer t;
+/*
+void count() {
+ auxOut1 = 1;
+}
+*/
+
+
int main() {
+// pulse.fall(&count);
+
cw.mode(PullUp);
ccw.mode(PullUp);
aux1.mode(PullUp);
+pulse.mode(PullUp);
+//auxOut1 = 1;
+
+servo1.period(0.02); // 2019-08-26
+servo2.period(0.02); // 2019-08-26
+
t.start();
+lastPulse = t.read_ms();
//aux1.rise(&triggered);
-servo2 = 0;
+//servo2 = 0;
@@ -142,7 +185,53 @@
aux1State = 0;
//wait(0.1);
}
-
+
+//********************** Pulse **********************************
+
+
+ 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 **********************************
@@ -159,6 +248,19 @@
if (c == 'W') { // wiggle setting
index = 5;
}
+
+ 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') {
@@ -179,6 +281,35 @@
}
}
+//******************************
+
+
+ if (index == 6) {
+ c = display.getc();
+ if (c == '0') {
+ n1Dir = 0;
+ index = 0;
+ }
+ if (c == '1') {
+ n1Dir = 1;
+ index = 0;
+ }
+ }
+
+ if (index == 7) {
+ c = display.getc();
+ if (c == '0') {
+ n2Dir = 0;
+ index = 0;
+ }
+ if (c == '1') {
+ n2Dir = 1;
+ index = 0;
+ }
+
+ }
+
+//******************************
@@ -197,13 +328,13 @@
index = 3;
}
- if (c == '3') { //N2 // Display adding 390 for N2 so 300 to 480 would be -90 to +90
+ else 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
+ else if (c == '4') { //N2
currentNozzle = 2;
Hundreds = 1;
index = 3;
@@ -325,11 +456,19 @@
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 (currentNozzle == 2){
+n2Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
+twitchStart = 0; // trigger sweep from new angle
+servo2Pos = servo2Scale.from(n2Pos);
+lastN2Called = servo2Pos;
+//servo1.pulsewidth_us(servo1Pos);
+index=0;
+}
}
@@ -337,68 +476,185 @@
- if (twitch == 0) {
- if (servo1Pos != lastServo1Pos) {
+ if (twitch == 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;
+ lastServo1Pos = servo1Pos;
}
+
+ if (servo2Pos != lastServo2Pos) {
+ twitchStart = 0;
+ twitchDirection = 0;
+ servo2Pos = servo1Scale.from(n2Pos);
+ servo2.pulsewidth_us(servo2Pos);
+ lastServo2Pos = servo2Pos;
}
+ }
+
if (twitch > 0) {
if (twitchStart == 0) {
//tw.start(); // start timer for nozzle sweep
//servo1Pos = servo1Scale.from(n1Pos);
servo1.pulsewidth_us(servo1Pos);
+ servo2.pulsewidth_us(servo2Pos);
lastTwitchMove = t.read_ms(); // store time of last nozzle movement
- minTwitch = servo1Pos; // store original servo postion for bottom of sweep
+ minTwitch1 = lastN1Called; // store original servo postion for bottom of sweep
+ minTwitch2 = lastN2Called; // store original servo postion for bottom of sweep
+
+ if(n1Dir == 0){
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.
+ 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) {
- maxTwitch = servo1Pos - 150;
+ maxTwitch1 = lastN1Called + 150;
+ }
+ if (twitch == 3) {
+ maxTwitch1 = lastN1Called + 200;
+ }
+ }
+
+ if(n2Dir == 0){
+ if (twitch == 1) {
+ maxTwitch2 = lastN2Called - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
+ }
+ if (twitch == 2) {
+ maxTwitch2 = lastN2Called - 150;
+ }
+ if (twitch == 3) {
+ maxTwitch2 = lastN2Called - 200;
+ }
+ }
+
+ if(n2Dir == 1){ // reverse
+ if (twitch == 1) {
+ maxTwitch2 = lastN2Called + 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
+ }
+ if (twitch == 2) {
+ maxTwitch2 = lastN2Called + 150;
}
if (twitch == 3) {
- maxTwitch = servo1Pos - 200;
+ maxTwitch2 = lastN2Called + 200;
+ }
}
- twitchStart = 1;
+ 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 (n1Dir ==0){
+ servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
+ }
+ if (n2Dir == 0){
+ servo2Pos = servo2Pos - twitchIncrement;
+ }
+
+ if (n1Dir ==1){
+ servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
+ }
+ if (n2Dir == 1){
+ servo2Pos = servo2Pos + twitchIncrement;
+ }
+
- if (servo1Pos < minServo) {
- servo1Pos = minServo;
+ if (n1Dir ==0 && servo1Pos < maxTwitch1 ) {
+ servo1Pos = maxTwitch1;
+ servo2Pos = maxTwitch2;
twitchDirection = 1; //reverse direction
}
- if (servo1Pos < maxTwitch) {
- servo1Pos = maxTwitch;
+
+ // if (n2Dir == 0 && servo2Pos < maxTwitch2) {
+ // servo2Pos = maxTwitch2;
+ // twitchDirection = 1; //reverse direction
+ // }
+ if (n1Dir ==1 && servo1Pos > maxTwitch1) {
+ servo1Pos = maxTwitch1;
+ servo2Pos = maxTwitch2;
twitchDirection = 1; //reverse direction
}
+
+ // if (n2Dir == 1 && servo2Pos > maxTwitch2) {
+ // servo2Pos = maxTwitch2;
+ // 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;
+
+ if (n1Dir ==0){
+ servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
+ }
+ if (n2Dir == 0){
+ servo2Pos = servo2Pos + twitchIncrement;
+ }
+
+ if (n1Dir ==1){
+ servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
+ }
+ if (n2Dir == 1){
+ servo2Pos = servo2Pos - twitchIncrement;
+ }
+
+ /*
+ if (n1Dir == 0 && servo1Pos < minServo) {
+ servo1Pos = minServo;
twitchDirection = 0; //reverse direction
}
- if (servo1Pos > minTwitch) {
- servo1Pos = minTwitch;
+ */
+ if (n1Dir == 0 && servo1Pos > (minTwitch1+50)) {
+ servo1Pos = minTwitch1;
+ servo2Pos = minTwitch2;
+ twitchDirection = 0; //reverse direction
+ }
+ if (n1Dir == 1 && servo1Pos < (minTwitch1 - 50)) {
+ servo1Pos = minTwitch1;
+ servo2Pos = minTwitch2;
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 (servo2Pos != lastServo2Pos) {
+ servo2.pulsewidth_us(servo2Pos);
+ lastServo2Pos = servo2Pos;
+ }
+
+
}
@@ -419,7 +675,7 @@
if (c == 'T'){
index = 1;
- servo2=1;
+ // servo2=1;
}
@@ -429,7 +685,7 @@
if (c=='0'){
Tens = 0;
- servo2=1;
+ // servo2=1;
}
else if (c=='1'){