Peter Betz / Mbed 2 deprecated accuBlast_inputs_DUAL

DUAL inputs code

Dependencies:   mbed reScale USBDevice

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

Committer:
BETZtechnik
Date:
Sun Oct 06 21:18:30 2019 +0000
Revision:
25:baedb9e32b6d
Parent:
10:a3ada529b264 
twitch hold working

Who changed what in which revision?

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