Mark and Peter / Mbed 2 deprecated 11u68Displaytest

Remote inc coolant nozzle control program

Dependencies:   mbed reScale USBDevice

Dependents:   Nozzle_inputs Nozzle_display

main.cpp@25:5c71b4d95638, 2019-10-06 (annotated)

Committer:
BETZtechnik
Date:
Sun Oct 06 21:18:14 2019 +0000
Revision:
25:5c71b4d95638
Parent:
24:55fafbb82a37 
twitch hold working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BETZtechnik 25:5c71b4d95638 1 #include "mbed.h"
BETZtechnik 2:79c03ac27b72 2
BETZtechnik 0:09419d572e90 3 #include "reScale.h"
BETZtechnik 2:79c03ac27b72 4
BETZtechnik 24:55fafbb82a37 5 // DUAL NOZZLE VERSION 1.2/ 1.3 only!!
BETZtechnik 2:79c03ac27b72 6
BETZtechnik 2:79c03ac27b72 7 //Serial pc(USBTX, USBRX);
BETZtechnik 0:09419d572e90 8
BETZtechnik 5:18c6ff1370d0 9 //InterruptIn aux1(P0_7, PullUp); // index pulse
BETZtechnik 0:09419d572e90 10
BETZtechnik 5:18c6ff1370d0 11 DigitalIn aux1(P0_7);
BETZtechnik 25:5c71b4d95638 12 DigitalIn aux2(P1_21);
BETZtechnik 25:5c71b4d95638 13 DigitalIn cw(P1_24);
BETZtechnik 5:18c6ff1370d0 14 DigitalIn ccw(P2_7);
BETZtechnik 25:5c71b4d95638 15 DigitalIn pulse(P2_15);
BETZtechnik 24:55fafbb82a37 16 //InterruptIn pulse(P1_21);
BETZtechnik 5:18c6ff1370d0 17
BETZtechnik 5:18c6ff1370d0 18 int cwRequested = 0;
BETZtechnik 5:18c6ff1370d0 19 int ccwRequested = 0;
BETZtechnik 5:18c6ff1370d0 20
BETZtechnik 24:55fafbb82a37 21 int pulseActive = 0;
BETZtechnik 24:55fafbb82a37 22 int pulseFlag =1; // is pulse high or low. (off is high after the opto)
BETZtechnik 25:5c71b4d95638 23 int pulseStart = 0;
BETZtechnik 25:5c71b4d95638 24 int pulseCount = 0;
BETZtechnik 25:5c71b4d95638 25 int lastPulse = 0; // for debounce
BETZtechnik 25:5c71b4d95638 26 int pulseDebounce = 1; //ms for debounce pulse signal
BETZtechnik 24:55fafbb82a37 27
BETZtechnik 5:18c6ff1370d0 28 int aux1State = 0;
BETZtechnik 5:18c6ff1370d0 29
BETZtechnik 25:5c71b4d95638 30 int autoMode = 0; // setting for AUTO mode (X7a, Pulse etc.)
BETZtechnik 25:5c71b4d95638 31 int aux2Mode =0; // aux 2 use, twitch wait, etc.
BETZtechnik 25:5c71b4d95638 32 int twitchWaitFlag = 0; // was twitch wait enabled?
BETZtechnik 25:5c71b4d95638 33 int waitFlag = 0;
BETZtechnik 25:5c71b4d95638 34
BETZtechnik 5:18c6ff1370d0 35
BETZtechnik 5:18c6ff1370d0 36 DigitalOut dir(P0_15);
BETZtechnik 24:55fafbb82a37 37 DigitalOut auxOut1(P0_8);
BETZtechnik 5:18c6ff1370d0 38
BETZtechnik 8:6bb07a893f53 39 Serial display(P0_14, P0_13, 19200); // RS 485 TX RX?
BETZtechnik 8:6bb07a893f53 40 Serial inputs(P1_23,P0_20, 9600); //rs232 TX RX
BETZtechnik 8:6bb07a893f53 41
BETZtechnik 8:6bb07a893f53 42
BETZtechnik 21:db507362ca7a 43 int maxServo = 2500;
BETZtechnik 21:db507362ca7a 44 int minServo = 500;
BETZtechnik 22:a3ada529b264 45 reScale servo1Scale(90,-90,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
BETZtechnik 24:55fafbb82a37 46 reScale servo2Scale(90,-90,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
BETZtechnik 0:09419d572e90 47
BETZtechnik 24:55fafbb82a37 48
BETZtechnik 25:5c71b4d95638 49 PwmOut servo1(P2_2); //display board
BETZtechnik 25:5c71b4d95638 50 PwmOut servo2(P2_19);
BETZtechnik 0:09419d572e90 51
BETZtechnik 2:79c03ac27b72 52 //char tens = '0';
BETZtechnik 2:79c03ac27b72 53 //char ones = '1';
BETZtechnik 0:09419d572e90 54
BETZtechnik 20:8aebc19d9d1d 55 int Hundreds = 0;
BETZtechnik 25:5c71b4d95638 56 int Tens = 0;
BETZtechnik 25:5c71b4d95638 57 int Ones = 0;
BETZtechnik 25:5c71b4d95638 58
BETZtechnik 25:5c71b4d95638 59 int twitch = 0;
BETZtechnik 25:5c71b4d95638 60 int twitchStart=0;
BETZtechnik 25:5c71b4d95638 61 int twitchDirection=0;
BETZtechnik 25:5c71b4d95638 62 int lastTwitchMove = 0;
BETZtechnik 25:5c71b4d95638 63 int maxTwitch1 = 0;
BETZtechnik 25:5c71b4d95638 64 int maxTwitch2 = 0;
BETZtechnik 25:5c71b4d95638 65 int minTwitch1 = 0;
BETZtechnik 25:5c71b4d95638 66 int minTwitch2 = 0;
BETZtechnik 25:5c71b4d95638 67 int tReadMs = 5;
BETZtechnik 25:5c71b4d95638 68 int twitchIncrement = 10;
BETZtechnik 25:5c71b4d95638 69
BETZtechnik 25:5c71b4d95638 70
BETZtechnik 25:5c71b4d95638 71
BETZtechnik 25:5c71b4d95638 72 int currentNozzle = 0;
BETZtechnik 0:09419d572e90 73
BETZtechnik 2:79c03ac27b72 74 int index = 0; // for parsing serial
BETZtechnik 0:09419d572e90 75
BETZtechnik 2:79c03ac27b72 76 int n1Pos = 0;
BETZtechnik 24:55fafbb82a37 77 int n2Pos = 0;
BETZtechnik 24:55fafbb82a37 78
BETZtechnik 24:55fafbb82a37 79 int lastN1Called = 0; // store the last actual tool position sent from the display because twitch code can shift it.
BETZtechnik 24:55fafbb82a37 80 int lastN2Called = 0;
BETZtechnik 24:55fafbb82a37 81
BETZtechnik 24:55fafbb82a37 82 int n1Dir = 0;
BETZtechnik 24:55fafbb82a37 83 int n2Dir = 0;
BETZtechnik 0:09419d572e90 84
BETZtechnik 25:5c71b4d95638 85 int servo1Pos = 0;
BETZtechnik 25:5c71b4d95638 86 int lastServo1Pos = 0;
BETZtechnik 25:5c71b4d95638 87
BETZtechnik 25:5c71b4d95638 88 int servo2Pos = 0;
BETZtechnik 25:5c71b4d95638 89 int lastServo2Pos = 0;
BETZtechnik 0:09419d572e90 90
BETZtechnik 5:18c6ff1370d0 91
BETZtechnik 24:55fafbb82a37 92 //DigitalOut servo2(P2_2);
BETZtechnik 0:09419d572e90 93
BETZtechnik 8:6bb07a893f53 94 /*
BETZtechnik 5:18c6ff1370d0 95 void triggered()
BETZtechnik 5:18c6ff1370d0 96 {
BETZtechnik 5:18c6ff1370d0 97 servo2 = 1;
BETZtechnik 25:5c71b4d95638 98
BETZtechnik 5:18c6ff1370d0 99 if (cw == 0){
BETZtechnik 5:18c6ff1370d0 100 cwRequested = 1;
BETZtechnik 5:18c6ff1370d0 101 }
BETZtechnik 25:5c71b4d95638 102
BETZtechnik 5:18c6ff1370d0 103 else if (ccw == 0){
BETZtechnik 5:18c6ff1370d0 104 ccwRequested = 1;
BETZtechnik 5:18c6ff1370d0 105 }
BETZtechnik 25:5c71b4d95638 106
BETZtechnik 25:5c71b4d95638 107 }
BETZtechnik 25:5c71b4d95638 108
BETZtechnik 8:6bb07a893f53 109 */
BETZtechnik 5:18c6ff1370d0 110
BETZtechnik 21:db507362ca7a 111 Timer t;
BETZtechnik 5:18c6ff1370d0 112
BETZtechnik 24:55fafbb82a37 113 /*
BETZtechnik 24:55fafbb82a37 114 void count() {
BETZtechnik 24:55fafbb82a37 115 auxOut1 = 1;
BETZtechnik 24:55fafbb82a37 116 }
BETZtechnik 24:55fafbb82a37 117 */
BETZtechnik 24:55fafbb82a37 118
BETZtechnik 24:55fafbb82a37 119
BETZtechnik 25:5c71b4d95638 120 int main()
BETZtechnik 25:5c71b4d95638 121 {
BETZtechnik 25:5c71b4d95638 122
BETZtechnik 25:5c71b4d95638 123 // pulse.fall(&count);
BETZtechnik 25:5c71b4d95638 124
BETZtechnik 25:5c71b4d95638 125 cw.mode(PullUp);
BETZtechnik 25:5c71b4d95638 126 ccw.mode(PullUp);
BETZtechnik 25:5c71b4d95638 127 aux1.mode(PullUp);
BETZtechnik 25:5c71b4d95638 128 pulse.mode(PullUp);
BETZtechnik 25:5c71b4d95638 129 aux2.mode(PullUp);
BETZtechnik 24:55fafbb82a37 130 //auxOut1 = 1;
BETZtechnik 24:55fafbb82a37 131
BETZtechnik 25:5c71b4d95638 132 servo1.period(0.02); // 2019-08-26
BETZtechnik 25:5c71b4d95638 133 servo2.period(0.02); // 2019-08-26
BETZtechnik 24:55fafbb82a37 134
BETZtechnik 2:79c03ac27b72 135
BETZtechnik 25:5c71b4d95638 136 t.start();
BETZtechnik 8:6bb07a893f53 137
BETZtechnik 25:5c71b4d95638 138 lastPulse = t.read_ms();
BETZtechnik 8:6bb07a893f53 139
BETZtechnik 5:18c6ff1370d0 140 //aux1.rise(&triggered);
BETZtechnik 5:18c6ff1370d0 141
BETZtechnik 5:18c6ff1370d0 142
BETZtechnik 5:18c6ff1370d0 143
BETZtechnik 24:55fafbb82a37 144 //servo2 = 0;
BETZtechnik 2:79c03ac27b72 145
BETZtechnik 2:79c03ac27b72 146
BETZtechnik 2:79c03ac27b72 147
BETZtechnik 25:5c71b4d95638 148 uint8_t c = 0; // for 485 link
BETZtechnik 25:5c71b4d95638 149
BETZtechnik 2:79c03ac27b72 150 while(1) {
BETZtechnik 25:5c71b4d95638 151
BETZtechnik 25:5c71b4d95638 152
BETZtechnik 25:5c71b4d95638 153 if (t.read() > 1200) {
BETZtechnik 25:5c71b4d95638 154 t.reset();
BETZtechnik 25:5c71b4d95638 155 lastTwitchMove = t.read_ms();
BETZtechnik 25:5c71b4d95638 156 lastPulse = t.read_ms();
BETZtechnik 25:5c71b4d95638 157 }
BETZtechnik 25:5c71b4d95638 158
BETZtechnik 25:5c71b4d95638 159
BETZtechnik 25:5c71b4d95638 160 //************************************ AUTO MODES **********************
BETZtechnik 25:5c71b4d95638 161
BETZtechnik 25:5c71b4d95638 162 if (autoMode == 1) { // ATC pin monitoring mode
BETZtechnik 25:5c71b4d95638 163 if (aux1 == 0 && aux1State == 0) {
BETZtechnik 25:5c71b4d95638 164
BETZtechnik 25:5c71b4d95638 165 if (cw == 1) {
BETZtechnik 5:18c6ff1370d0 166 //servo2 = 1;
BETZtechnik 25:5c71b4d95638 167 dir= 1;
BETZtechnik 5:18c6ff1370d0 168 //wait(0.001);
BETZtechnik 25:5c71b4d95638 169 display.printf("M");
BETZtechnik 25:5c71b4d95638 170 wait(0.05);
BETZtechnik 25:5c71b4d95638 171 display.printf("-");
BETZtechnik 25:5c71b4d95638 172 wait(0.001);
BETZtechnik 25:5c71b4d95638 173 dir=0;
BETZtechnik 25:5c71b4d95638 174 aux1State = 1;
BETZtechnik 5:18c6ff1370d0 175 //wait(.001);
BETZtechnik 25:5c71b4d95638 176 }
BETZtechnik 5:18c6ff1370d0 177
BETZtechnik 25:5c71b4d95638 178 if (ccw == 1) {
BETZtechnik 25:5c71b4d95638 179 dir= 1;
BETZtechnik 5:18c6ff1370d0 180 //wait(0.01);
BETZtechnik 25:5c71b4d95638 181 display.printf("M");
BETZtechnik 25:5c71b4d95638 182 wait(0.05);
BETZtechnik 25:5c71b4d95638 183 display.printf("+");
BETZtechnik 25:5c71b4d95638 184 wait(0.001);
BETZtechnik 25:5c71b4d95638 185 dir=0;
BETZtechnik 25:5c71b4d95638 186 aux1State = 1;
BETZtechnik 5:18c6ff1370d0 187 //wait(.001);
BETZtechnik 25:5c71b4d95638 188 }
BETZtechnik 25:5c71b4d95638 189 }
BETZtechnik 5:18c6ff1370d0 190
BETZtechnik 25:5c71b4d95638 191 if (aux1 == 1 && aux1State == 1) {
BETZtechnik 25:5c71b4d95638 192 aux1State = 0;
BETZtechnik 5:18c6ff1370d0 193 //wait(0.1);
BETZtechnik 25:5c71b4d95638 194 }
BETZtechnik 25:5c71b4d95638 195 } //end of if ATC pin monitoring
BETZtechnik 25:5c71b4d95638 196
BETZtechnik 25:5c71b4d95638 197
BETZtechnik 25:5c71b4d95638 198 //********************** Pulse **********************************
BETZtechnik 25:5c71b4d95638 199
BETZtechnik 25:5c71b4d95638 200
BETZtechnik 25:5c71b4d95638 201
BETZtechnik 25:5c71b4d95638 202 if (autoMode == 2) { // pulse mode
BETZtechnik 25:5c71b4d95638 203 if (pulse == 0 && pulseActive == 0 && pulseFlag == 1 && (t.read_ms() > (lastPulse + pulseDebounce))) {
BETZtechnik 25:5c71b4d95638 204 pulseStart = t.read_ms();
BETZtechnik 25:5c71b4d95638 205 lastPulse = t.read_ms();
BETZtechnik 25:5c71b4d95638 206 pulseActive = 1;
BETZtechnik 25:5c71b4d95638 207 pulseCount = 1;
BETZtechnik 25:5c71b4d95638 208 pulseFlag = 0; // pin is now low
BETZtechnik 24:55fafbb82a37 209 //wait(0.01);
BETZtechnik 25:5c71b4d95638 210 }
BETZtechnik 24:55fafbb82a37 211
BETZtechnik 25:5c71b4d95638 212 if (pulse == 1 && pulseActive == 1 && pulseFlag == 0 && (t.read_ms() > lastPulse + pulseDebounce)) {
BETZtechnik 25:5c71b4d95638 213 pulseFlag = 1;
BETZtechnik 25:5c71b4d95638 214 lastPulse = t.read_ms();
BETZtechnik 25:5c71b4d95638 215 }
BETZtechnik 24:55fafbb82a37 216
BETZtechnik 25:5c71b4d95638 217 if (pulse == 0 && pulseActive == 1 && pulseFlag == 1 && (t.read_ms() > lastPulse + pulseDebounce)) {
BETZtechnik 25:5c71b4d95638 218 pulseCount = pulseCount + 1;
BETZtechnik 25:5c71b4d95638 219 pulseFlag =0;
BETZtechnik 25:5c71b4d95638 220 lastPulse = t.read_ms();
BETZtechnik 24:55fafbb82a37 221 //wait(0.01);
BETZtechnik 25:5c71b4d95638 222 }
BETZtechnik 24:55fafbb82a37 223
BETZtechnik 25:5c71b4d95638 224 if (pulseActive == 1 && (t.read_ms() > pulseStart + 1000)) {
BETZtechnik 25:5c71b4d95638 225 pulseCount = pulseCount + 100;
BETZtechnik 24:55fafbb82a37 226
BETZtechnik 25:5c71b4d95638 227 dir=1;
BETZtechnik 25:5c71b4d95638 228 display.printf("T");
BETZtechnik 25:5c71b4d95638 229 wait(0.05);
BETZtechnik 25:5c71b4d95638 230 display.printf("%d", pulseCount);
BETZtechnik 25:5c71b4d95638 231 wait(0.001);
BETZtechnik 25:5c71b4d95638 232 dir=0;
BETZtechnik 24:55fafbb82a37 233
BETZtechnik 25:5c71b4d95638 234 pulseCount = 0;
BETZtechnik 25:5c71b4d95638 235 pulseActive = 0;
BETZtechnik 25:5c71b4d95638 236 pulseFlag=1;
BETZtechnik 25:5c71b4d95638 237 lastPulse = t.read_ms();
BETZtechnik 25:5c71b4d95638 238 //wait(0.01);
BETZtechnik 25:5c71b4d95638 239 }
BETZtechnik 25:5c71b4d95638 240 } // end of if pulse mode
BETZtechnik 25:5c71b4d95638 241
BETZtechnik 25:5c71b4d95638 242
BETZtechnik 20:8aebc19d9d1d 243
BETZtechnik 20:8aebc19d9d1d 244 //********************** Display code NEW with W **********************************
BETZtechnik 25:5c71b4d95638 245 //**********************STAGE 1
BETZtechnik 20:8aebc19d9d1d 246
BETZtechnik 20:8aebc19d9d1d 247 while (display.readable()) {
BETZtechnik 20:8aebc19d9d1d 248
BETZtechnik 20:8aebc19d9d1d 249 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 250
BETZtechnik 20:8aebc19d9d1d 251 if (c == 'N') {
BETZtechnik 20:8aebc19d9d1d 252
BETZtechnik 20:8aebc19d9d1d 253 index = 1;
BETZtechnik 20:8aebc19d9d1d 254
BETZtechnik 20:8aebc19d9d1d 255 }
BETZtechnik 20:8aebc19d9d1d 256
BETZtechnik 20:8aebc19d9d1d 257 if (c == 'W') { // wiggle setting
BETZtechnik 20:8aebc19d9d1d 258 index = 5;
BETZtechnik 20:8aebc19d9d1d 259 }
BETZtechnik 25:5c71b4d95638 260
BETZtechnik 25:5c71b4d95638 261 if (c == 'A') { // auto mode
BETZtechnik 25:5c71b4d95638 262 index = 8;
BETZtechnik 25:5c71b4d95638 263 }
BETZtechnik 25:5c71b4d95638 264
BETZtechnik 25:5c71b4d95638 265 if (c == 'I') { // aux2 input mode
BETZtechnik 25:5c71b4d95638 266 index = 9;
BETZtechnik 25:5c71b4d95638 267 }
BETZtechnik 25:5c71b4d95638 268
BETZtechnik 25:5c71b4d95638 269 if (c == 'D') { // nozzle direction
BETZtechnik 25:5c71b4d95638 270 c = display.getc();
BETZtechnik 25:5c71b4d95638 271 if (c == '1') { // N1
BETZtechnik 25:5c71b4d95638 272 index = 6;
BETZtechnik 24:55fafbb82a37 273 }
BETZtechnik 25:5c71b4d95638 274 if (c == '2') { // N2
BETZtechnik 25:5c71b4d95638 275 index = 7;
BETZtechnik 25:5c71b4d95638 276 }
BETZtechnik 25:5c71b4d95638 277 }
BETZtechnik 25:5c71b4d95638 278
BETZtechnik 25:5c71b4d95638 279
BETZtechnik 25:5c71b4d95638 280
BETZtechnik 25:5c71b4d95638 281
BETZtechnik 25:5c71b4d95638 282 //**********************STAGE 2
BETZtechnik 25:5c71b4d95638 283
BETZtechnik 25:5c71b4d95638 284
BETZtechnik 25:5c71b4d95638 285
BETZtechnik 25:5c71b4d95638 286
BETZtechnik 20:8aebc19d9d1d 287 if (index == 5) {
BETZtechnik 20:8aebc19d9d1d 288 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 289 if (c == '0') {
BETZtechnik 20:8aebc19d9d1d 290 twitch = 0;
BETZtechnik 20:8aebc19d9d1d 291 index = 0;
BETZtechnik 20:8aebc19d9d1d 292 }
BETZtechnik 20:8aebc19d9d1d 293 if (c == '1') {
BETZtechnik 20:8aebc19d9d1d 294 twitch = 1;
BETZtechnik 21:db507362ca7a 295 index = 0;
BETZtechnik 20:8aebc19d9d1d 296 }
BETZtechnik 20:8aebc19d9d1d 297 if (c == '2') {
BETZtechnik 20:8aebc19d9d1d 298 twitch = 2;
BETZtechnik 20:8aebc19d9d1d 299 index = 0;
BETZtechnik 20:8aebc19d9d1d 300 }
BETZtechnik 20:8aebc19d9d1d 301 if (c == '3') {
BETZtechnik 20:8aebc19d9d1d 302 twitch = 3;
BETZtechnik 20:8aebc19d9d1d 303 index = 0;
BETZtechnik 20:8aebc19d9d1d 304 }
BETZtechnik 20:8aebc19d9d1d 305 }
BETZtechnik 20:8aebc19d9d1d 306
BETZtechnik 24:55fafbb82a37 307 //******************************
BETZtechnik 24:55fafbb82a37 308
BETZtechnik 24:55fafbb82a37 309
BETZtechnik 24:55fafbb82a37 310 if (index == 6) {
BETZtechnik 24:55fafbb82a37 311 c = display.getc();
BETZtechnik 24:55fafbb82a37 312 if (c == '0') {
BETZtechnik 24:55fafbb82a37 313 n1Dir = 0;
BETZtechnik 24:55fafbb82a37 314 index = 0;
BETZtechnik 24:55fafbb82a37 315 }
BETZtechnik 24:55fafbb82a37 316 if (c == '1') {
BETZtechnik 24:55fafbb82a37 317 n1Dir = 1;
BETZtechnik 24:55fafbb82a37 318 index = 0;
BETZtechnik 25:5c71b4d95638 319 }
BETZtechnik 24:55fafbb82a37 320 }
BETZtechnik 25:5c71b4d95638 321
BETZtechnik 25:5c71b4d95638 322 if (index == 7) {
BETZtechnik 24:55fafbb82a37 323 c = display.getc();
BETZtechnik 24:55fafbb82a37 324 if (c == '0') {
BETZtechnik 24:55fafbb82a37 325 n2Dir = 0;
BETZtechnik 24:55fafbb82a37 326 index = 0;
BETZtechnik 24:55fafbb82a37 327 }
BETZtechnik 24:55fafbb82a37 328 if (c == '1') {
BETZtechnik 24:55fafbb82a37 329 n2Dir = 1;
BETZtechnik 24:55fafbb82a37 330 index = 0;
BETZtechnik 25:5c71b4d95638 331 }
BETZtechnik 25:5c71b4d95638 332
BETZtechnik 25:5c71b4d95638 333 }
BETZtechnik 25:5c71b4d95638 334
BETZtechnik 25:5c71b4d95638 335 if (index == 8) { // auto mode
BETZtechnik 25:5c71b4d95638 336 c = display.getc();
BETZtechnik 25:5c71b4d95638 337 if (c == '1') {
BETZtechnik 25:5c71b4d95638 338 autoMode = 1; // X7a (x7 atc)
BETZtechnik 25:5c71b4d95638 339 index = 0;
BETZtechnik 25:5c71b4d95638 340 }
BETZtechnik 25:5c71b4d95638 341 if (c == '2') {
BETZtechnik 25:5c71b4d95638 342 autoMode = 2; // Pulse
BETZtechnik 25:5c71b4d95638 343 index = 0;
BETZtechnik 25:5c71b4d95638 344 }
BETZtechnik 25:5c71b4d95638 345
BETZtechnik 25:5c71b4d95638 346 }
BETZtechnik 24:55fafbb82a37 347
BETZtechnik 25:5c71b4d95638 348 if (index == 9) { // aux2 mode
BETZtechnik 25:5c71b4d95638 349 c = display.getc();
BETZtechnik 25:5c71b4d95638 350 if (c == '0') {
BETZtechnik 25:5c71b4d95638 351 aux2Mode = 0; // 0= off, 1 = twitch pause wait for coolant signal
BETZtechnik 25:5c71b4d95638 352 index = 0;
BETZtechnik 25:5c71b4d95638 353 }
BETZtechnik 25:5c71b4d95638 354 if (c == '1') {
BETZtechnik 25:5c71b4d95638 355 aux2Mode = 1; // twitch pause
BETZtechnik 25:5c71b4d95638 356 index = 0;
BETZtechnik 25:5c71b4d95638 357 }
BETZtechnik 25:5c71b4d95638 358
BETZtechnik 25:5c71b4d95638 359 }
BETZtechnik 25:5c71b4d95638 360
BETZtechnik 25:5c71b4d95638 361
BETZtechnik 24:55fafbb82a37 362
BETZtechnik 24:55fafbb82a37 363 //******************************
BETZtechnik 20:8aebc19d9d1d 364
BETZtechnik 20:8aebc19d9d1d 365
BETZtechnik 20:8aebc19d9d1d 366
BETZtechnik 20:8aebc19d9d1d 367 if (index == 1) { //Display is sending -90>+90 and adding 190 for N1 so would send 100 to 280 for -90 to +90
BETZtechnik 20:8aebc19d9d1d 368 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 369 if (c == '1') { // N1
BETZtechnik 20:8aebc19d9d1d 370 currentNozzle = 1;
BETZtechnik 20:8aebc19d9d1d 371 Hundreds = 0;
BETZtechnik 20:8aebc19d9d1d 372 index = 3;
BETZtechnik 20:8aebc19d9d1d 373
BETZtechnik 25:5c71b4d95638 374 }
BETZtechnik 20:8aebc19d9d1d 375
BETZtechnik 25:5c71b4d95638 376 else if (c == '2') { //N2
BETZtechnik 20:8aebc19d9d1d 377 currentNozzle = 1;
BETZtechnik 20:8aebc19d9d1d 378 Hundreds = 1;
BETZtechnik 20:8aebc19d9d1d 379 index = 3;
BETZtechnik 20:8aebc19d9d1d 380 }
BETZtechnik 20:8aebc19d9d1d 381
BETZtechnik 25:5c71b4d95638 382 else if (c == '3') { //N2 // Display adding 390 for N2 so 300 to 480 would be -90 to +90
BETZtechnik 20:8aebc19d9d1d 383 currentNozzle = 2;
BETZtechnik 20:8aebc19d9d1d 384 Hundreds = 0;
BETZtechnik 20:8aebc19d9d1d 385 index = 3;
BETZtechnik 20:8aebc19d9d1d 386 }
BETZtechnik 20:8aebc19d9d1d 387
BETZtechnik 25:5c71b4d95638 388 else if (c == '4') { //N2
BETZtechnik 20:8aebc19d9d1d 389 currentNozzle = 2;
BETZtechnik 20:8aebc19d9d1d 390 Hundreds = 1;
BETZtechnik 20:8aebc19d9d1d 391 index = 3;
BETZtechnik 20:8aebc19d9d1d 392 }
BETZtechnik 20:8aebc19d9d1d 393
BETZtechnik 20:8aebc19d9d1d 394
BETZtechnik 20:8aebc19d9d1d 395 }
BETZtechnik 20:8aebc19d9d1d 396
BETZtechnik 20:8aebc19d9d1d 397
BETZtechnik 20:8aebc19d9d1d 398
BETZtechnik 20:8aebc19d9d1d 399 if (index == 3) {
BETZtechnik 20:8aebc19d9d1d 400
BETZtechnik 20:8aebc19d9d1d 401 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 402
BETZtechnik 20:8aebc19d9d1d 403 if (c=='0') {
BETZtechnik 20:8aebc19d9d1d 404 Tens = 0;
BETZtechnik 20:8aebc19d9d1d 405 index = 4;
BETZtechnik 20:8aebc19d9d1d 406
BETZtechnik 20:8aebc19d9d1d 407 }
BETZtechnik 20:8aebc19d9d1d 408
BETZtechnik 20:8aebc19d9d1d 409 else if (c=='1') {
BETZtechnik 20:8aebc19d9d1d 410 Tens = 1;
BETZtechnik 20:8aebc19d9d1d 411 index = 4;
BETZtechnik 20:8aebc19d9d1d 412
BETZtechnik 20:8aebc19d9d1d 413 }
BETZtechnik 20:8aebc19d9d1d 414
BETZtechnik 20:8aebc19d9d1d 415 else if (c=='2') {
BETZtechnik 20:8aebc19d9d1d 416 Tens = 2;
BETZtechnik 20:8aebc19d9d1d 417 index = 4;
BETZtechnik 20:8aebc19d9d1d 418
BETZtechnik 20:8aebc19d9d1d 419 }
BETZtechnik 20:8aebc19d9d1d 420
BETZtechnik 20:8aebc19d9d1d 421 else if (c=='3') {
BETZtechnik 20:8aebc19d9d1d 422 Tens = 3;
BETZtechnik 20:8aebc19d9d1d 423 index = 4;
BETZtechnik 20:8aebc19d9d1d 424
BETZtechnik 20:8aebc19d9d1d 425 }
BETZtechnik 20:8aebc19d9d1d 426
BETZtechnik 20:8aebc19d9d1d 427 else if (c=='4') {
BETZtechnik 20:8aebc19d9d1d 428 Tens = 4;
BETZtechnik 20:8aebc19d9d1d 429 index = 4;
BETZtechnik 20:8aebc19d9d1d 430 }
BETZtechnik 20:8aebc19d9d1d 431
BETZtechnik 20:8aebc19d9d1d 432 else if (c=='5') {
BETZtechnik 20:8aebc19d9d1d 433 Tens = 5;
BETZtechnik 20:8aebc19d9d1d 434 index = 4;
BETZtechnik 20:8aebc19d9d1d 435 }
BETZtechnik 20:8aebc19d9d1d 436
BETZtechnik 20:8aebc19d9d1d 437 else if (c=='6') {
BETZtechnik 20:8aebc19d9d1d 438 Tens = 6;
BETZtechnik 20:8aebc19d9d1d 439 index = 4;
BETZtechnik 20:8aebc19d9d1d 440 }
BETZtechnik 20:8aebc19d9d1d 441
BETZtechnik 20:8aebc19d9d1d 442 else if (c=='7') {
BETZtechnik 20:8aebc19d9d1d 443 Tens = 7;
BETZtechnik 20:8aebc19d9d1d 444 index = 4;
BETZtechnik 25:5c71b4d95638 445 }
BETZtechnik 25:5c71b4d95638 446
BETZtechnik 20:8aebc19d9d1d 447 else if (c=='8') {
BETZtechnik 20:8aebc19d9d1d 448 Tens = 8;
BETZtechnik 20:8aebc19d9d1d 449 index = 4;
BETZtechnik 20:8aebc19d9d1d 450 }
BETZtechnik 20:8aebc19d9d1d 451
BETZtechnik 20:8aebc19d9d1d 452 else if (c=='9') {
BETZtechnik 20:8aebc19d9d1d 453 Tens = 9;
BETZtechnik 20:8aebc19d9d1d 454 index = 4;
BETZtechnik 20:8aebc19d9d1d 455 }
BETZtechnik 20:8aebc19d9d1d 456
BETZtechnik 20:8aebc19d9d1d 457
BETZtechnik 20:8aebc19d9d1d 458 }
BETZtechnik 20:8aebc19d9d1d 459
BETZtechnik 20:8aebc19d9d1d 460 if (index == 4) {
BETZtechnik 20:8aebc19d9d1d 461
BETZtechnik 20:8aebc19d9d1d 462 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 463
BETZtechnik 20:8aebc19d9d1d 464 if (c=='0') {
BETZtechnik 20:8aebc19d9d1d 465 Ones = 0;
BETZtechnik 20:8aebc19d9d1d 466 }
BETZtechnik 20:8aebc19d9d1d 467
BETZtechnik 20:8aebc19d9d1d 468 else if (c=='1') {
BETZtechnik 20:8aebc19d9d1d 469 Ones = 1;
BETZtechnik 20:8aebc19d9d1d 470 }
BETZtechnik 20:8aebc19d9d1d 471
BETZtechnik 20:8aebc19d9d1d 472 else if (c=='2') {
BETZtechnik 20:8aebc19d9d1d 473 Ones = 2;
BETZtechnik 20:8aebc19d9d1d 474 }
BETZtechnik 20:8aebc19d9d1d 475
BETZtechnik 20:8aebc19d9d1d 476 else if (c=='3') {
BETZtechnik 20:8aebc19d9d1d 477 Ones = 3;
BETZtechnik 20:8aebc19d9d1d 478 }
BETZtechnik 20:8aebc19d9d1d 479
BETZtechnik 20:8aebc19d9d1d 480 else if (c=='4') {
BETZtechnik 20:8aebc19d9d1d 481 Ones = 4;
BETZtechnik 20:8aebc19d9d1d 482 }
BETZtechnik 20:8aebc19d9d1d 483
BETZtechnik 20:8aebc19d9d1d 484 else if (c=='5') {
BETZtechnik 20:8aebc19d9d1d 485 Ones = 5;
BETZtechnik 20:8aebc19d9d1d 486 }
BETZtechnik 20:8aebc19d9d1d 487
BETZtechnik 20:8aebc19d9d1d 488 else if (c=='6') {
BETZtechnik 20:8aebc19d9d1d 489 Ones = 6;
BETZtechnik 20:8aebc19d9d1d 490 }
BETZtechnik 20:8aebc19d9d1d 491
BETZtechnik 20:8aebc19d9d1d 492 else if (c=='7') {
BETZtechnik 20:8aebc19d9d1d 493 Ones = 7;
BETZtechnik 20:8aebc19d9d1d 494 }
BETZtechnik 20:8aebc19d9d1d 495
BETZtechnik 20:8aebc19d9d1d 496 else if (c=='8') {
BETZtechnik 20:8aebc19d9d1d 497 Ones = 8;
BETZtechnik 20:8aebc19d9d1d 498 }
BETZtechnik 20:8aebc19d9d1d 499
BETZtechnik 20:8aebc19d9d1d 500 else if (c=='9') {
BETZtechnik 20:8aebc19d9d1d 501 Ones = 9;
BETZtechnik 20:8aebc19d9d1d 502 }
BETZtechnik 20:8aebc19d9d1d 503
BETZtechnik 25:5c71b4d95638 504
BETZtechnik 25:5c71b4d95638 505
BETZtechnik 25:5c71b4d95638 506 if (currentNozzle == 1) {
BETZtechnik 25:5c71b4d95638 507 n1Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
BETZtechnik 25:5c71b4d95638 508 twitchStart = 0; // trigger sweep from new angle
BETZtechnik 25:5c71b4d95638 509 servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 25:5c71b4d95638 510 lastN1Called = servo1Pos;
BETZtechnik 25:5c71b4d95638 511 //servo1.pulsewidth_us(servo1Pos);
BETZtechnik 25:5c71b4d95638 512 index=0;
BETZtechnik 25:5c71b4d95638 513 }
BETZtechnik 25:5c71b4d95638 514
BETZtechnik 25:5c71b4d95638 515 if (currentNozzle == 2) {
BETZtechnik 25:5c71b4d95638 516 n2Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
BETZtechnik 25:5c71b4d95638 517 twitchStart = 0; // trigger sweep from new angle
BETZtechnik 25:5c71b4d95638 518 servo2Pos = servo2Scale.from(n2Pos);
BETZtechnik 25:5c71b4d95638 519 lastN2Called = servo2Pos;
BETZtechnik 25:5c71b4d95638 520 //servo1.pulsewidth_us(servo1Pos);
BETZtechnik 25:5c71b4d95638 521 index=0;
BETZtechnik 25:5c71b4d95638 522 }
BETZtechnik 25:5c71b4d95638 523
BETZtechnik 25:5c71b4d95638 524
BETZtechnik 20:8aebc19d9d1d 525 }
BETZtechnik 20:8aebc19d9d1d 526 }
BETZtechnik 21:db507362ca7a 527
BETZtechnik 21:db507362ca7a 528
BETZtechnik 25:5c71b4d95638 529
BETZtechnik 25:5c71b4d95638 530 if ((twitch == 0) || (twitch > 0 && aux2Mode == 1 && aux2 == 1)) {
BETZtechnik 24:55fafbb82a37 531 if (servo1Pos != lastServo1Pos) {
BETZtechnik 21:db507362ca7a 532 twitchStart = 0;
BETZtechnik 21:db507362ca7a 533 twitchDirection = 0;
BETZtechnik 21:db507362ca7a 534 servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 535 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 24:55fafbb82a37 536 lastServo1Pos = servo1Pos;
BETZtechnik 21:db507362ca7a 537 }
BETZtechnik 25:5c71b4d95638 538
BETZtechnik 24:55fafbb82a37 539 if (servo2Pos != lastServo2Pos) {
BETZtechnik 24:55fafbb82a37 540 twitchStart = 0;
BETZtechnik 24:55fafbb82a37 541 twitchDirection = 0;
BETZtechnik 24:55fafbb82a37 542 servo2Pos = servo1Scale.from(n2Pos);
BETZtechnik 24:55fafbb82a37 543 servo2.pulsewidth_us(servo2Pos);
BETZtechnik 24:55fafbb82a37 544 lastServo2Pos = servo2Pos;
BETZtechnik 21:db507362ca7a 545 }
BETZtechnik 21:db507362ca7a 546
BETZtechnik 24:55fafbb82a37 547 }
BETZtechnik 24:55fafbb82a37 548
BETZtechnik 25:5c71b4d95638 549 if (twitch > 0 && aux2Mode != 1 || (twitch > 0 && aux2Mode == 1 && aux2 == 0)) {
BETZtechnik 25:5c71b4d95638 550 if (twitchStart == 0) {
BETZtechnik 25:5c71b4d95638 551 //tw.start(); // start timer for nozzle sweep
BETZtechnik 25:5c71b4d95638 552 //servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 25:5c71b4d95638 553 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 25:5c71b4d95638 554 servo2.pulsewidth_us(servo2Pos);
BETZtechnik 25:5c71b4d95638 555 lastTwitchMove = t.read_ms(); // store time of last nozzle movement
BETZtechnik 25:5c71b4d95638 556 minTwitch1 = lastN1Called; // store original servo postion for bottom of sweep
BETZtechnik 25:5c71b4d95638 557 minTwitch2 = lastN2Called; // store original servo postion for bottom of sweep
BETZtechnik 25:5c71b4d95638 558
BETZtechnik 25:5c71b4d95638 559 if(n1Dir == 0) {
BETZtechnik 21:db507362ca7a 560 if (twitch == 1) {
BETZtechnik 24:55fafbb82a37 561 maxTwitch1 = lastN1Called - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
BETZtechnik 24:55fafbb82a37 562 }
BETZtechnik 24:55fafbb82a37 563 if (twitch == 2) {
BETZtechnik 24:55fafbb82a37 564 maxTwitch1 = lastN1Called - 150;
BETZtechnik 24:55fafbb82a37 565 }
BETZtechnik 24:55fafbb82a37 566 if (twitch == 3) {
BETZtechnik 24:55fafbb82a37 567 maxTwitch1 = lastN1Called - 200;
BETZtechnik 24:55fafbb82a37 568 }
BETZtechnik 25:5c71b4d95638 569 }
BETZtechnik 25:5c71b4d95638 570
BETZtechnik 25:5c71b4d95638 571 if(n1Dir == 1) { // reverse
BETZtechnik 24:55fafbb82a37 572 if (twitch == 1) {
BETZtechnik 24:55fafbb82a37 573 maxTwitch1 = lastN1Called + 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
BETZtechnik 21:db507362ca7a 574 }
BETZtechnik 21:db507362ca7a 575 if (twitch == 2) {
BETZtechnik 24:55fafbb82a37 576 maxTwitch1 = lastN1Called + 150;
BETZtechnik 24:55fafbb82a37 577 }
BETZtechnik 24:55fafbb82a37 578 if (twitch == 3) {
BETZtechnik 24:55fafbb82a37 579 maxTwitch1 = lastN1Called + 200;
BETZtechnik 24:55fafbb82a37 580 }
BETZtechnik 25:5c71b4d95638 581 }
BETZtechnik 25:5c71b4d95638 582
BETZtechnik 25:5c71b4d95638 583 if(n2Dir == 0) {
BETZtechnik 24:55fafbb82a37 584 if (twitch == 1) {
BETZtechnik 24:55fafbb82a37 585 maxTwitch2 = lastN2Called - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
BETZtechnik 24:55fafbb82a37 586 }
BETZtechnik 24:55fafbb82a37 587 if (twitch == 2) {
BETZtechnik 24:55fafbb82a37 588 maxTwitch2 = lastN2Called - 150;
BETZtechnik 24:55fafbb82a37 589 }
BETZtechnik 24:55fafbb82a37 590 if (twitch == 3) {
BETZtechnik 24:55fafbb82a37 591 maxTwitch2 = lastN2Called - 200;
BETZtechnik 24:55fafbb82a37 592 }
BETZtechnik 25:5c71b4d95638 593 }
BETZtechnik 25:5c71b4d95638 594
BETZtechnik 25:5c71b4d95638 595 if(n2Dir == 1) { // reverse
BETZtechnik 24:55fafbb82a37 596 if (twitch == 1) {
BETZtechnik 24:55fafbb82a37 597 maxTwitch2 = lastN2Called + 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
BETZtechnik 24:55fafbb82a37 598 }
BETZtechnik 24:55fafbb82a37 599 if (twitch == 2) {
BETZtechnik 24:55fafbb82a37 600 maxTwitch2 = lastN2Called + 150;
BETZtechnik 21:db507362ca7a 601 }
BETZtechnik 21:db507362ca7a 602 if (twitch == 3) {
BETZtechnik 24:55fafbb82a37 603 maxTwitch2 = lastN2Called + 200;
BETZtechnik 24:55fafbb82a37 604 }
BETZtechnik 25:5c71b4d95638 605 }
BETZtechnik 25:5c71b4d95638 606
BETZtechnik 25:5c71b4d95638 607 twitchStart = 1;
BETZtechnik 25:5c71b4d95638 608
BETZtechnik 25:5c71b4d95638 609 }
BETZtechnik 25:5c71b4d95638 610
BETZtechnik 25:5c71b4d95638 611
BETZtechnik 25:5c71b4d95638 612 if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
BETZtechnik 25:5c71b4d95638 613 if (twitchDirection == 0) { //going up
BETZtechnik 25:5c71b4d95638 614 if (n1Dir ==0) {
BETZtechnik 25:5c71b4d95638 615 servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
BETZtechnik 21:db507362ca7a 616 }
BETZtechnik 25:5c71b4d95638 617 if (n2Dir == 0) {
BETZtechnik 24:55fafbb82a37 618 servo2Pos = servo2Pos - twitchIncrement;
BETZtechnik 25:5c71b4d95638 619 }
BETZtechnik 25:5c71b4d95638 620
BETZtechnik 25:5c71b4d95638 621 if (n1Dir ==1) {
BETZtechnik 24:55fafbb82a37 622 servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
BETZtechnik 25:5c71b4d95638 623 }
BETZtechnik 25:5c71b4d95638 624 if (n2Dir == 1) {
BETZtechnik 24:55fafbb82a37 625 servo2Pos = servo2Pos + twitchIncrement;
BETZtechnik 25:5c71b4d95638 626 }
BETZtechnik 25:5c71b4d95638 627
BETZtechnik 21:db507362ca7a 628
BETZtechnik 25:5c71b4d95638 629 if (n1Dir ==0 && servo1Pos < maxTwitch1 ) {
BETZtechnik 25:5c71b4d95638 630 servo1Pos = maxTwitch1;
BETZtechnik 25:5c71b4d95638 631 servo2Pos = maxTwitch2;
BETZtechnik 25:5c71b4d95638 632 twitchDirection = 1; //reverse direction
BETZtechnik 25:5c71b4d95638 633 }
BETZtechnik 25:5c71b4d95638 634
BETZtechnik 25:5c71b4d95638 635 // if (n2Dir == 0 && servo2Pos < maxTwitch2) {
BETZtechnik 25:5c71b4d95638 636 // servo2Pos = maxTwitch2;
BETZtechnik 25:5c71b4d95638 637 // twitchDirection = 1; //reverse direction
BETZtechnik 25:5c71b4d95638 638 // }
BETZtechnik 25:5c71b4d95638 639 if (n1Dir ==1 && servo1Pos > maxTwitch1) {
BETZtechnik 25:5c71b4d95638 640 servo1Pos = maxTwitch1;
BETZtechnik 25:5c71b4d95638 641 servo2Pos = maxTwitch2;
BETZtechnik 25:5c71b4d95638 642 twitchDirection = 1; //reverse direction
BETZtechnik 25:5c71b4d95638 643 }
BETZtechnik 25:5c71b4d95638 644
BETZtechnik 24:55fafbb82a37 645 // if (n2Dir == 1 && servo2Pos > maxTwitch2) {
BETZtechnik 24:55fafbb82a37 646 // servo2Pos = maxTwitch2;
BETZtechnik 25:5c71b4d95638 647 // twitchDirection = 1; //reverse direction
BETZtechnik 25:5c71b4d95638 648 // }
BETZtechnik 25:5c71b4d95638 649
BETZtechnik 25:5c71b4d95638 650
BETZtechnik 25:5c71b4d95638 651 lastTwitchMove = t.read_ms();
BETZtechnik 25:5c71b4d95638 652 }
BETZtechnik 25:5c71b4d95638 653
BETZtechnik 25:5c71b4d95638 654 if (twitchDirection == 1) { // going down
BETZtechnik 25:5c71b4d95638 655
BETZtechnik 25:5c71b4d95638 656 if (n1Dir ==0) {
BETZtechnik 25:5c71b4d95638 657 servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
BETZtechnik 25:5c71b4d95638 658 }
BETZtechnik 25:5c71b4d95638 659 if (n2Dir == 0) {
BETZtechnik 25:5c71b4d95638 660 servo2Pos = servo2Pos + twitchIncrement;
BETZtechnik 21:db507362ca7a 661 }
BETZtechnik 21:db507362ca7a 662
BETZtechnik 25:5c71b4d95638 663 if (n1Dir ==1) {
BETZtechnik 24:55fafbb82a37 664 servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
BETZtechnik 25:5c71b4d95638 665 }
BETZtechnik 25:5c71b4d95638 666 if (n2Dir == 1) {
BETZtechnik 24:55fafbb82a37 667 servo2Pos = servo2Pos - twitchIncrement;
BETZtechnik 25:5c71b4d95638 668 }
BETZtechnik 25:5c71b4d95638 669
BETZtechnik 24:55fafbb82a37 670 /*
BETZtechnik 24:55fafbb82a37 671 if (n1Dir == 0 && servo1Pos < minServo) {
BETZtechnik 24:55fafbb82a37 672 servo1Pos = minServo;
BETZtechnik 21:db507362ca7a 673 twitchDirection = 0; //reverse direction
BETZtechnik 21:db507362ca7a 674 }
BETZtechnik 24:55fafbb82a37 675 */
BETZtechnik 25:5c71b4d95638 676 if (n1Dir == 0 && servo1Pos > (minTwitch1+50)) {
BETZtechnik 25:5c71b4d95638 677 servo1Pos = minTwitch1;
BETZtechnik 25:5c71b4d95638 678 servo2Pos = minTwitch2;
BETZtechnik 25:5c71b4d95638 679 twitchDirection = 0; //reverse direction
BETZtechnik 25:5c71b4d95638 680 }
BETZtechnik 25:5c71b4d95638 681 if (n1Dir == 1 && servo1Pos < (minTwitch1 - 50)) {
BETZtechnik 25:5c71b4d95638 682 servo1Pos = minTwitch1;
BETZtechnik 25:5c71b4d95638 683 servo2Pos = minTwitch2;
BETZtechnik 25:5c71b4d95638 684 twitchDirection = 0; //reverse direction
BETZtechnik 21:db507362ca7a 685 }
BETZtechnik 25:5c71b4d95638 686 // if (n2Dir == 0 && servo2Pos > (minTwitch2+0)) {
BETZtechnik 25:5c71b4d95638 687 // servo2Pos = minTwitch2;
BETZtechnik 25:5c71b4d95638 688 // twitchDirection = 0; //reverse direction
BETZtechnik 25:5c71b4d95638 689 // }
BETZtechnik 25:5c71b4d95638 690 // if (n2Dir == 1 && servo2Pos < minTwitch2) {
BETZtechnik 25:5c71b4d95638 691 // servo2Pos = minTwitch2;
BETZtechnik 25:5c71b4d95638 692 // twitchDirection = 0; //reverse direction
BETZtechnik 25:5c71b4d95638 693 // }
BETZtechnik 25:5c71b4d95638 694
BETZtechnik 25:5c71b4d95638 695 lastTwitchMove = t.read_ms();
BETZtechnik 21:db507362ca7a 696 }
BETZtechnik 25:5c71b4d95638 697
BETZtechnik 25:5c71b4d95638 698 if (servo1Pos != lastServo1Pos) {
BETZtechnik 25:5c71b4d95638 699 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 25:5c71b4d95638 700 lastServo1Pos = servo1Pos;
BETZtechnik 25:5c71b4d95638 701 }
BETZtechnik 25:5c71b4d95638 702
BETZtechnik 25:5c71b4d95638 703 if (servo2Pos != lastServo2Pos) {
BETZtechnik 25:5c71b4d95638 704 servo2.pulsewidth_us(servo2Pos);
BETZtechnik 25:5c71b4d95638 705 lastServo2Pos = servo2Pos;
BETZtechnik 25:5c71b4d95638 706 }
BETZtechnik 20:8aebc19d9d1d 707
BETZtechnik 20:8aebc19d9d1d 708
BETZtechnik 25:5c71b4d95638 709 }
BETZtechnik 25:5c71b4d95638 710 }
BETZtechnik 25:5c71b4d95638 711
BETZtechnik 25:5c71b4d95638 712 if (twitch >0 && aux2Mode == 1 && aux2 == 1) {
BETZtechnik 25:5c71b4d95638 713 if (waitFlag == 0) {
BETZtechnik 25:5c71b4d95638 714 dir=1;
BETZtechnik 25:5c71b4d95638 715 display.printf("X");
BETZtechnik 25:5c71b4d95638 716 wait(0.05);
BETZtechnik 25:5c71b4d95638 717 display.printf("X");
BETZtechnik 25:5c71b4d95638 718 wait(0.001);
BETZtechnik 25:5c71b4d95638 719 dir=0;
BETZtechnik 25:5c71b4d95638 720 waitFlag =1;
BETZtechnik 25:5c71b4d95638 721 // wait (0.1);
BETZtechnik 25:5c71b4d95638 722 }
BETZtechnik 25:5c71b4d95638 723 }
BETZtechnik 25:5c71b4d95638 724 if ((twitch == 0) || (twitch >0 && aux2Mode == 1 && aux2 == 0)|| (twitch >0 && aux2Mode == 0)) {
BETZtechnik 25:5c71b4d95638 725 if (waitFlag == 1) {
BETZtechnik 25:5c71b4d95638 726 dir=1;
BETZtechnik 25:5c71b4d95638 727 display.printf("Y");
BETZtechnik 25:5c71b4d95638 728 wait(0.05);
BETZtechnik 25:5c71b4d95638 729 display.printf("Y");
BETZtechnik 25:5c71b4d95638 730 wait(0.001);
BETZtechnik 25:5c71b4d95638 731 dir=0;
BETZtechnik 25:5c71b4d95638 732 waitFlag =0;
BETZtechnik 25:5c71b4d95638 733 // wait (0.1);
BETZtechnik 25:5c71b4d95638 734 }
BETZtechnik 25:5c71b4d95638 735 }
BETZtechnik 0:09419d572e90 736
BETZtechnik 0:09419d572e90 737
BETZtechnik 8:6bb07a893f53 738 //********************************* Inputs serial *******************************
BETZtechnik 8:6bb07a893f53 739
BETZtechnik 8:6bb07a893f53 740
BETZtechnik 25:5c71b4d95638 741 while (inputs.readable()) {
BETZtechnik 25:5c71b4d95638 742
BETZtechnik 25:5c71b4d95638 743
BETZtechnik 25:5c71b4d95638 744 c = inputs.getc();
BETZtechnik 25:5c71b4d95638 745
BETZtechnik 25:5c71b4d95638 746 if (c == 'T') {
BETZtechnik 8:6bb07a893f53 747
BETZtechnik 25:5c71b4d95638 748 index = 1;
BETZtechnik 25:5c71b4d95638 749 // servo2=1;
BETZtechnik 25:5c71b4d95638 750 }
BETZtechnik 25:5c71b4d95638 751
BETZtechnik 25:5c71b4d95638 752
BETZtechnik 25:5c71b4d95638 753 if (index == 1) {
BETZtechnik 25:5c71b4d95638 754 c = inputs.getc();
BETZtechnik 25:5c71b4d95638 755
BETZtechnik 25:5c71b4d95638 756
BETZtechnik 25:5c71b4d95638 757 if (c=='0') {
BETZtechnik 25:5c71b4d95638 758 Tens = 0;
BETZtechnik 24:55fafbb82a37 759 // servo2=1;
BETZtechnik 25:5c71b4d95638 760 }
BETZtechnik 25:5c71b4d95638 761
BETZtechnik 25:5c71b4d95638 762 else if (c=='1') {
BETZtechnik 25:5c71b4d95638 763 Tens = 1;
BETZtechnik 25:5c71b4d95638 764
BETZtechnik 25:5c71b4d95638 765
BETZtechnik 25:5c71b4d95638 766 }
BETZtechnik 25:5c71b4d95638 767
BETZtechnik 25:5c71b4d95638 768 else if (c=='2') {
BETZtechnik 25:5c71b4d95638 769 Tens = 2;
BETZtechnik 25:5c71b4d95638 770
BETZtechnik 25:5c71b4d95638 771 }
BETZtechnik 25:5c71b4d95638 772
BETZtechnik 25:5c71b4d95638 773 else if (c=='3') {
BETZtechnik 25:5c71b4d95638 774 Tens = 3;
BETZtechnik 25:5c71b4d95638 775
BETZtechnik 25:5c71b4d95638 776 }
BETZtechnik 25:5c71b4d95638 777
BETZtechnik 25:5c71b4d95638 778 else if (c=='4') {
BETZtechnik 25:5c71b4d95638 779 Tens = 4;
BETZtechnik 25:5c71b4d95638 780 }
BETZtechnik 25:5c71b4d95638 781
BETZtechnik 25:5c71b4d95638 782 else if (c=='5') {
BETZtechnik 25:5c71b4d95638 783 Tens = 5;
BETZtechnik 25:5c71b4d95638 784 }
BETZtechnik 25:5c71b4d95638 785
BETZtechnik 25:5c71b4d95638 786 else if (c=='6') {
BETZtechnik 25:5c71b4d95638 787 Tens = 6;
BETZtechnik 25:5c71b4d95638 788 }
BETZtechnik 25:5c71b4d95638 789
BETZtechnik 25:5c71b4d95638 790 else if (c=='7') {
BETZtechnik 25:5c71b4d95638 791 Tens = 7;
BETZtechnik 25:5c71b4d95638 792 }
BETZtechnik 25:5c71b4d95638 793
BETZtechnik 25:5c71b4d95638 794 else if (c=='8') {
BETZtechnik 25:5c71b4d95638 795 Tens = 8;
BETZtechnik 25:5c71b4d95638 796 }
BETZtechnik 25:5c71b4d95638 797
BETZtechnik 25:5c71b4d95638 798 else if (c=='9') {
BETZtechnik 25:5c71b4d95638 799 Tens = 9;
BETZtechnik 25:5c71b4d95638 800 }
BETZtechnik 25:5c71b4d95638 801
BETZtechnik 25:5c71b4d95638 802 else if (c=='0') {
BETZtechnik 25:5c71b4d95638 803 Tens = 0;
BETZtechnik 25:5c71b4d95638 804 }
BETZtechnik 25:5c71b4d95638 805 index = 2;
BETZtechnik 25:5c71b4d95638 806 }
BETZtechnik 25:5c71b4d95638 807
BETZtechnik 25:5c71b4d95638 808
BETZtechnik 25:5c71b4d95638 809 if (index == 2) {
BETZtechnik 25:5c71b4d95638 810 c = inputs.getc();
BETZtechnik 25:5c71b4d95638 811 if (c=='0') {
BETZtechnik 25:5c71b4d95638 812 Ones = 0;
BETZtechnik 25:5c71b4d95638 813
BETZtechnik 25:5c71b4d95638 814 }
BETZtechnik 25:5c71b4d95638 815
BETZtechnik 25:5c71b4d95638 816 else if (c=='1') {
BETZtechnik 25:5c71b4d95638 817 Ones = 1;
BETZtechnik 25:5c71b4d95638 818
BETZtechnik 25:5c71b4d95638 819
BETZtechnik 25:5c71b4d95638 820 }
BETZtechnik 25:5c71b4d95638 821
BETZtechnik 25:5c71b4d95638 822 else if (c=='2') {
BETZtechnik 25:5c71b4d95638 823 Ones = 2;
BETZtechnik 25:5c71b4d95638 824
BETZtechnik 25:5c71b4d95638 825 }
BETZtechnik 25:5c71b4d95638 826
BETZtechnik 25:5c71b4d95638 827 else if (c=='3') {
BETZtechnik 25:5c71b4d95638 828 Ones = 3;
BETZtechnik 25:5c71b4d95638 829
BETZtechnik 25:5c71b4d95638 830 }
BETZtechnik 25:5c71b4d95638 831
BETZtechnik 25:5c71b4d95638 832 else if (c=='4') {
BETZtechnik 25:5c71b4d95638 833 Ones = 4;
BETZtechnik 25:5c71b4d95638 834 }
BETZtechnik 25:5c71b4d95638 835
BETZtechnik 25:5c71b4d95638 836 else if (c=='5') {
BETZtechnik 25:5c71b4d95638 837 Ones = 5;
BETZtechnik 25:5c71b4d95638 838 }
BETZtechnik 25:5c71b4d95638 839
BETZtechnik 25:5c71b4d95638 840 else if (c=='6') {
BETZtechnik 25:5c71b4d95638 841 Ones = 6;
BETZtechnik 25:5c71b4d95638 842 }
BETZtechnik 25:5c71b4d95638 843
BETZtechnik 25:5c71b4d95638 844 else if (c=='7') {
BETZtechnik 25:5c71b4d95638 845 Ones = 7;
BETZtechnik 25:5c71b4d95638 846 }
BETZtechnik 25:5c71b4d95638 847
BETZtechnik 25:5c71b4d95638 848 else if (c=='8') {
BETZtechnik 25:5c71b4d95638 849 Ones = 8;
BETZtechnik 25:5c71b4d95638 850 }
BETZtechnik 25:5c71b4d95638 851
BETZtechnik 25:5c71b4d95638 852 else if (c=='9') {
BETZtechnik 25:5c71b4d95638 853 Ones = 9;
BETZtechnik 25:5c71b4d95638 854 }
BETZtechnik 25:5c71b4d95638 855
BETZtechnik 25:5c71b4d95638 856 else if (c=='0') {
BETZtechnik 25:5c71b4d95638 857 Ones = 0;
BETZtechnik 25:5c71b4d95638 858 }
BETZtechnik 25:5c71b4d95638 859 int tempToolNo = (Tens*10)+100+Ones;
BETZtechnik 25:5c71b4d95638 860 dir=1;
BETZtechnik 25:5c71b4d95638 861 display.printf("T");
BETZtechnik 25:5c71b4d95638 862 wait(0.05);
BETZtechnik 25:5c71b4d95638 863 display.printf("%d", tempToolNo);
BETZtechnik 25:5c71b4d95638 864 wait(0.001);
BETZtechnik 25:5c71b4d95638 865 dir=0;
BETZtechnik 25:5c71b4d95638 866 index = 0;
BETZtechnik 25:5c71b4d95638 867 tempToolNo=0;
BETZtechnik 25:5c71b4d95638 868
BETZtechnik 25:5c71b4d95638 869 }
BETZtechnik 25:5c71b4d95638 870
BETZtechnik 25:5c71b4d95638 871
BETZtechnik 25:5c71b4d95638 872 }
BETZtechnik 8:6bb07a893f53 873
BETZtechnik 8:6bb07a893f53 874
BETZtechnik 8:6bb07a893f53 875 // *****************************************************************************
BETZtechnik 5:18c6ff1370d0 876
BETZtechnik 0:09419d572e90 877
BETZtechnik 2:79c03ac27b72 878
BETZtechnik 25:5c71b4d95638 879 }
BETZtechnik 2:79c03ac27b72 880 }