Peter Betz / Mbed 2 deprecated accuBlast_inputs

SINGLE Inputs code

Dependencies:   mbed reScale USBDevice

main.cpp@24:633485c12ef1, 2019-10-06 (annotated)

Committer:
BETZtechnik
Date:
Sun Oct 06 21:58:29 2019 +0000
Revision:
24:633485c12ef1
Parent:
11:baedb9e32b6d 
fixed x7atc

Who changed what in which revision?

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