Mark and Peter / Mbed 2 deprecated 11u68Displaytest

Remote inc coolant nozzle control program

Dependencies:   mbed reScale USBDevice

Dependents:   Nozzle_inputs Nozzle_display

main.cpp@24:55fafbb82a37, 2019-09-15 (annotated)

Committer:
BETZtechnik
Date:
Sun Sep 15 18:40:26 2019 +0000
Revision:
24:55fafbb82a37
Parent:
22:a3ada529b264
Child:
25:5c71b4d95638
New twitch code, N1/ N2 direction activated. Tool pulse working!

Who changed what in which revision?

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