Mark and Peter / Mbed 2 deprecated 11u68Displaytest

Remote inc coolant nozzle control program

Dependencies:   mbed reScale USBDevice

Dependents:   Nozzle_inputs Nozzle_display

main.cpp@21:db507362ca7a, 2019-04-10 (annotated)

Committer:
BETZtechnik
Date:
Wed Apr 10 23:23:13 2019 +0000
Revision:
21:db507362ca7a
Parent:
20:8aebc19d9d1d
Child:
22:a3ada529b264
Twitch added back in/ no freezing/ single nozzle

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 2:79c03ac27b72 5
BETZtechnik 2:79c03ac27b72 6 //Serial pc(USBTX, USBRX);
BETZtechnik 0:09419d572e90 7
BETZtechnik 5:18c6ff1370d0 8 //InterruptIn aux1(P0_7, PullUp); // index pulse
BETZtechnik 0:09419d572e90 9
BETZtechnik 5:18c6ff1370d0 10 DigitalIn aux1(P0_7);
BETZtechnik 5:18c6ff1370d0 11 DigitalIn cw(P1_24);
BETZtechnik 5:18c6ff1370d0 12 DigitalIn ccw(P2_7);
BETZtechnik 5:18c6ff1370d0 13
BETZtechnik 5:18c6ff1370d0 14 int cwRequested = 0;
BETZtechnik 5:18c6ff1370d0 15 int ccwRequested = 0;
BETZtechnik 5:18c6ff1370d0 16
BETZtechnik 5:18c6ff1370d0 17 int aux1State = 0;
BETZtechnik 5:18c6ff1370d0 18
BETZtechnik 5:18c6ff1370d0 19
BETZtechnik 5:18c6ff1370d0 20 DigitalOut dir(P0_15);
BETZtechnik 5:18c6ff1370d0 21
BETZtechnik 8:6bb07a893f53 22 Serial display(P0_14, P0_13, 19200); // RS 485 TX RX?
BETZtechnik 8:6bb07a893f53 23 Serial inputs(P1_23,P0_20, 9600); //rs232 TX RX
BETZtechnik 8:6bb07a893f53 24
BETZtechnik 8:6bb07a893f53 25
BETZtechnik 21:db507362ca7a 26 int maxServo = 2500;
BETZtechnik 21:db507362ca7a 27 int minServo = 500;
BETZtechnik 21:db507362ca7a 28 reScale servo1Scale(45,0,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
BETZtechnik 0:09419d572e90 29
BETZtechnik 5:18c6ff1370d0 30 PwmOut servo1(P1_13); //display board
BETZtechnik 0:09419d572e90 31
BETZtechnik 2:79c03ac27b72 32 //char tens = '0';
BETZtechnik 2:79c03ac27b72 33 //char ones = '1';
BETZtechnik 0:09419d572e90 34
BETZtechnik 20:8aebc19d9d1d 35 int Hundreds = 0;
BETZtechnik 2:79c03ac27b72 36 int Tens = 0;
BETZtechnik 2:79c03ac27b72 37 int Ones = 0;
BETZtechnik 2:79c03ac27b72 38
BETZtechnik 20:8aebc19d9d1d 39 int twitch = 0;
BETZtechnik 21:db507362ca7a 40 int twitchStart=0;
BETZtechnik 21:db507362ca7a 41 int twitchDirection=0;
BETZtechnik 21:db507362ca7a 42 int lastTwitchMove = 0;
BETZtechnik 21:db507362ca7a 43 int maxTwitch = 0;
BETZtechnik 21:db507362ca7a 44 int minTwitch = 0;
BETZtechnik 21:db507362ca7a 45 int tReadMs = 50;
BETZtechnik 20:8aebc19d9d1d 46
BETZtechnik 2:79c03ac27b72 47 int currentNozzle = 0;
BETZtechnik 0:09419d572e90 48
BETZtechnik 2:79c03ac27b72 49 int index = 0; // for parsing serial
BETZtechnik 0:09419d572e90 50
BETZtechnik 2:79c03ac27b72 51 int n1Pos = 0;
BETZtechnik 0:09419d572e90 52
BETZtechnik 2:79c03ac27b72 53 int servo1Pos = 0;
BETZtechnik 21:db507362ca7a 54 int lastServo1Pos = 999;
BETZtechnik 0:09419d572e90 55
BETZtechnik 5:18c6ff1370d0 56
BETZtechnik 2:79c03ac27b72 57 DigitalOut servo2(P2_2);
BETZtechnik 0:09419d572e90 58
BETZtechnik 8:6bb07a893f53 59 /*
BETZtechnik 5:18c6ff1370d0 60 void triggered()
BETZtechnik 5:18c6ff1370d0 61 {
BETZtechnik 5:18c6ff1370d0 62 servo2 = 1;
BETZtechnik 5:18c6ff1370d0 63
BETZtechnik 5:18c6ff1370d0 64 if (cw == 0){
BETZtechnik 5:18c6ff1370d0 65 cwRequested = 1;
BETZtechnik 5:18c6ff1370d0 66 }
BETZtechnik 5:18c6ff1370d0 67
BETZtechnik 5:18c6ff1370d0 68 else if (ccw == 0){
BETZtechnik 5:18c6ff1370d0 69 ccwRequested = 1;
BETZtechnik 5:18c6ff1370d0 70 }
BETZtechnik 5:18c6ff1370d0 71
BETZtechnik 5:18c6ff1370d0 72 }
BETZtechnik 5:18c6ff1370d0 73
BETZtechnik 8:6bb07a893f53 74 */
BETZtechnik 5:18c6ff1370d0 75
BETZtechnik 21:db507362ca7a 76 Timer t;
BETZtechnik 5:18c6ff1370d0 77
BETZtechnik 0:09419d572e90 78 int main() {
BETZtechnik 4:fa0af3a8e089 79
BETZtechnik 5:18c6ff1370d0 80 cw.mode(PullUp);
BETZtechnik 5:18c6ff1370d0 81 ccw.mode(PullUp);
BETZtechnik 5:18c6ff1370d0 82 aux1.mode(PullUp);
BETZtechnik 2:79c03ac27b72 83
BETZtechnik 21:db507362ca7a 84 t.start();
BETZtechnik 8:6bb07a893f53 85
BETZtechnik 8:6bb07a893f53 86
BETZtechnik 5:18c6ff1370d0 87 //aux1.rise(&triggered);
BETZtechnik 5:18c6ff1370d0 88
BETZtechnik 5:18c6ff1370d0 89
BETZtechnik 5:18c6ff1370d0 90
BETZtechnik 5:18c6ff1370d0 91 servo2 = 0;
BETZtechnik 2:79c03ac27b72 92
BETZtechnik 2:79c03ac27b72 93
BETZtechnik 2:79c03ac27b72 94
BETZtechnik 2:79c03ac27b72 95 uint8_t c = 0; // for 485 link
BETZtechnik 0:09419d572e90 96
BETZtechnik 2:79c03ac27b72 97 while(1) {
BETZtechnik 21:db507362ca7a 98
BETZtechnik 20:8aebc19d9d1d 99
BETZtechnik 21:db507362ca7a 100 if (t.read() > 1200){
BETZtechnik 21:db507362ca7a 101 t.reset();
BETZtechnik 21:db507362ca7a 102 lastTwitchMove = t.read_ms();
BETZtechnik 21:db507362ca7a 103 }
BETZtechnik 20:8aebc19d9d1d 104
BETZtechnik 4:fa0af3a8e089 105
BETZtechnik 5:18c6ff1370d0 106 //************************************ Interrupt response **********************
BETZtechnik 8:6bb07a893f53 107
BETZtechnik 5:18c6ff1370d0 108
BETZtechnik 8:6bb07a893f53 109 if (aux1 == 0 && aux1State == 0)
BETZtechnik 5:18c6ff1370d0 110 {
BETZtechnik 5:18c6ff1370d0 111
BETZtechnik 8:6bb07a893f53 112 if (cw == 1)
BETZtechnik 5:18c6ff1370d0 113 {
BETZtechnik 5:18c6ff1370d0 114 //servo2 = 1;
BETZtechnik 5:18c6ff1370d0 115 dir= 1;
BETZtechnik 5:18c6ff1370d0 116 //wait(0.001);
BETZtechnik 8:6bb07a893f53 117 display.printf("M");
BETZtechnik 5:18c6ff1370d0 118 wait(0.05);
BETZtechnik 8:6bb07a893f53 119 display.printf("-");
BETZtechnik 5:18c6ff1370d0 120 wait(0.001);
BETZtechnik 5:18c6ff1370d0 121 dir=0;
BETZtechnik 5:18c6ff1370d0 122 aux1State = 1;
BETZtechnik 5:18c6ff1370d0 123 //wait(.001);
BETZtechnik 5:18c6ff1370d0 124 }
BETZtechnik 5:18c6ff1370d0 125
BETZtechnik 8:6bb07a893f53 126 if (ccw == 1)
BETZtechnik 5:18c6ff1370d0 127 {
BETZtechnik 5:18c6ff1370d0 128 dir= 1;
BETZtechnik 5:18c6ff1370d0 129 //wait(0.01);
BETZtechnik 8:6bb07a893f53 130 display.printf("M");
BETZtechnik 5:18c6ff1370d0 131 wait(0.05);
BETZtechnik 8:6bb07a893f53 132 display.printf("+");
BETZtechnik 5:18c6ff1370d0 133 wait(0.001);
BETZtechnik 5:18c6ff1370d0 134 dir=0;
BETZtechnik 5:18c6ff1370d0 135 aux1State = 1;
BETZtechnik 5:18c6ff1370d0 136 //wait(.001);
BETZtechnik 5:18c6ff1370d0 137 }
BETZtechnik 5:18c6ff1370d0 138 }
BETZtechnik 5:18c6ff1370d0 139
BETZtechnik 8:6bb07a893f53 140 if (aux1 == 1 && aux1State == 1)
BETZtechnik 5:18c6ff1370d0 141 {
BETZtechnik 5:18c6ff1370d0 142 aux1State = 0;
BETZtechnik 5:18c6ff1370d0 143 //wait(0.1);
BETZtechnik 5:18c6ff1370d0 144 }
BETZtechnik 4:fa0af3a8e089 145
BETZtechnik 20:8aebc19d9d1d 146
BETZtechnik 20:8aebc19d9d1d 147 //********************** Display code NEW with W **********************************
BETZtechnik 20:8aebc19d9d1d 148
BETZtechnik 20:8aebc19d9d1d 149 while (display.readable()) {
BETZtechnik 20:8aebc19d9d1d 150
BETZtechnik 20:8aebc19d9d1d 151 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 152
BETZtechnik 20:8aebc19d9d1d 153 if (c == 'N') {
BETZtechnik 20:8aebc19d9d1d 154
BETZtechnik 20:8aebc19d9d1d 155 index = 1;
BETZtechnik 20:8aebc19d9d1d 156
BETZtechnik 20:8aebc19d9d1d 157 }
BETZtechnik 20:8aebc19d9d1d 158
BETZtechnik 20:8aebc19d9d1d 159 if (c == 'W') { // wiggle setting
BETZtechnik 20:8aebc19d9d1d 160 index = 5;
BETZtechnik 20:8aebc19d9d1d 161 }
BETZtechnik 20:8aebc19d9d1d 162 if (index == 5) {
BETZtechnik 20:8aebc19d9d1d 163 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 164 if (c == '0') {
BETZtechnik 20:8aebc19d9d1d 165 twitch = 0;
BETZtechnik 20:8aebc19d9d1d 166 index = 0;
BETZtechnik 20:8aebc19d9d1d 167 }
BETZtechnik 20:8aebc19d9d1d 168 if (c == '1') {
BETZtechnik 20:8aebc19d9d1d 169 twitch = 1;
BETZtechnik 21:db507362ca7a 170 index = 0;
BETZtechnik 20:8aebc19d9d1d 171 }
BETZtechnik 20:8aebc19d9d1d 172 if (c == '2') {
BETZtechnik 20:8aebc19d9d1d 173 twitch = 2;
BETZtechnik 20:8aebc19d9d1d 174 index = 0;
BETZtechnik 20:8aebc19d9d1d 175 }
BETZtechnik 20:8aebc19d9d1d 176 if (c == '3') {
BETZtechnik 20:8aebc19d9d1d 177 twitch = 3;
BETZtechnik 20:8aebc19d9d1d 178 index = 0;
BETZtechnik 20:8aebc19d9d1d 179 }
BETZtechnik 20:8aebc19d9d1d 180 }
BETZtechnik 20:8aebc19d9d1d 181
BETZtechnik 20:8aebc19d9d1d 182
BETZtechnik 20:8aebc19d9d1d 183
BETZtechnik 20:8aebc19d9d1d 184
BETZtechnik 20:8aebc19d9d1d 185 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 186 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 187 if (c == '1') { // N1
BETZtechnik 20:8aebc19d9d1d 188 currentNozzle = 1;
BETZtechnik 20:8aebc19d9d1d 189 Hundreds = 0;
BETZtechnik 20:8aebc19d9d1d 190 index = 3;
BETZtechnik 20:8aebc19d9d1d 191
BETZtechnik 20:8aebc19d9d1d 192
BETZtechnik 20:8aebc19d9d1d 193 } else if (c == '2') { //N2
BETZtechnik 20:8aebc19d9d1d 194 currentNozzle = 1;
BETZtechnik 20:8aebc19d9d1d 195 Hundreds = 1;
BETZtechnik 20:8aebc19d9d1d 196 index = 3;
BETZtechnik 20:8aebc19d9d1d 197 }
BETZtechnik 20:8aebc19d9d1d 198
BETZtechnik 20:8aebc19d9d1d 199 if (c == '3') { //N2 // Display adding 390 for N2 so 300 to 480 would be -90 to +90
BETZtechnik 20:8aebc19d9d1d 200 currentNozzle = 2;
BETZtechnik 20:8aebc19d9d1d 201 Hundreds = 0;
BETZtechnik 20:8aebc19d9d1d 202 index = 3;
BETZtechnik 20:8aebc19d9d1d 203 }
BETZtechnik 20:8aebc19d9d1d 204
BETZtechnik 20:8aebc19d9d1d 205 if (c == '4') { //N2
BETZtechnik 20:8aebc19d9d1d 206 currentNozzle = 2;
BETZtechnik 20:8aebc19d9d1d 207 Hundreds = 1;
BETZtechnik 20:8aebc19d9d1d 208 index = 3;
BETZtechnik 20:8aebc19d9d1d 209 }
BETZtechnik 20:8aebc19d9d1d 210
BETZtechnik 20:8aebc19d9d1d 211
BETZtechnik 20:8aebc19d9d1d 212 }
BETZtechnik 20:8aebc19d9d1d 213
BETZtechnik 20:8aebc19d9d1d 214
BETZtechnik 20:8aebc19d9d1d 215
BETZtechnik 20:8aebc19d9d1d 216 if (index == 3) {
BETZtechnik 20:8aebc19d9d1d 217
BETZtechnik 20:8aebc19d9d1d 218 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 219
BETZtechnik 20:8aebc19d9d1d 220 if (c=='0') {
BETZtechnik 20:8aebc19d9d1d 221 Tens = 0;
BETZtechnik 20:8aebc19d9d1d 222 index = 4;
BETZtechnik 20:8aebc19d9d1d 223
BETZtechnik 20:8aebc19d9d1d 224 }
BETZtechnik 20:8aebc19d9d1d 225
BETZtechnik 20:8aebc19d9d1d 226 else if (c=='1') {
BETZtechnik 20:8aebc19d9d1d 227 Tens = 1;
BETZtechnik 20:8aebc19d9d1d 228 index = 4;
BETZtechnik 20:8aebc19d9d1d 229
BETZtechnik 20:8aebc19d9d1d 230 }
BETZtechnik 20:8aebc19d9d1d 231
BETZtechnik 20:8aebc19d9d1d 232 else if (c=='2') {
BETZtechnik 20:8aebc19d9d1d 233 Tens = 2;
BETZtechnik 20:8aebc19d9d1d 234 index = 4;
BETZtechnik 20:8aebc19d9d1d 235
BETZtechnik 20:8aebc19d9d1d 236 }
BETZtechnik 20:8aebc19d9d1d 237
BETZtechnik 20:8aebc19d9d1d 238 else if (c=='3') {
BETZtechnik 20:8aebc19d9d1d 239 Tens = 3;
BETZtechnik 20:8aebc19d9d1d 240 index = 4;
BETZtechnik 20:8aebc19d9d1d 241
BETZtechnik 20:8aebc19d9d1d 242 }
BETZtechnik 20:8aebc19d9d1d 243
BETZtechnik 20:8aebc19d9d1d 244 else if (c=='4') {
BETZtechnik 20:8aebc19d9d1d 245 Tens = 4;
BETZtechnik 20:8aebc19d9d1d 246 index = 4;
BETZtechnik 20:8aebc19d9d1d 247 }
BETZtechnik 20:8aebc19d9d1d 248
BETZtechnik 20:8aebc19d9d1d 249 else if (c=='5') {
BETZtechnik 20:8aebc19d9d1d 250 Tens = 5;
BETZtechnik 20:8aebc19d9d1d 251 index = 4;
BETZtechnik 20:8aebc19d9d1d 252 }
BETZtechnik 20:8aebc19d9d1d 253
BETZtechnik 20:8aebc19d9d1d 254 else if (c=='6') {
BETZtechnik 20:8aebc19d9d1d 255 Tens = 6;
BETZtechnik 20:8aebc19d9d1d 256 index = 4;
BETZtechnik 20:8aebc19d9d1d 257 }
BETZtechnik 20:8aebc19d9d1d 258
BETZtechnik 20:8aebc19d9d1d 259 else if (c=='7') {
BETZtechnik 20:8aebc19d9d1d 260 Tens = 7;
BETZtechnik 20:8aebc19d9d1d 261 index = 4;
BETZtechnik 20:8aebc19d9d1d 262 }
BETZtechnik 20:8aebc19d9d1d 263
BETZtechnik 20:8aebc19d9d1d 264 else if (c=='8') {
BETZtechnik 20:8aebc19d9d1d 265 Tens = 8;
BETZtechnik 20:8aebc19d9d1d 266 index = 4;
BETZtechnik 20:8aebc19d9d1d 267 }
BETZtechnik 20:8aebc19d9d1d 268
BETZtechnik 20:8aebc19d9d1d 269 else if (c=='9') {
BETZtechnik 20:8aebc19d9d1d 270 Tens = 9;
BETZtechnik 20:8aebc19d9d1d 271 index = 4;
BETZtechnik 20:8aebc19d9d1d 272 }
BETZtechnik 20:8aebc19d9d1d 273
BETZtechnik 20:8aebc19d9d1d 274
BETZtechnik 20:8aebc19d9d1d 275 }
BETZtechnik 20:8aebc19d9d1d 276
BETZtechnik 20:8aebc19d9d1d 277 if (index == 4) {
BETZtechnik 20:8aebc19d9d1d 278
BETZtechnik 20:8aebc19d9d1d 279 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 280
BETZtechnik 20:8aebc19d9d1d 281 if (c=='0') {
BETZtechnik 20:8aebc19d9d1d 282 Ones = 0;
BETZtechnik 20:8aebc19d9d1d 283 }
BETZtechnik 20:8aebc19d9d1d 284
BETZtechnik 20:8aebc19d9d1d 285 else if (c=='1') {
BETZtechnik 20:8aebc19d9d1d 286 Ones = 1;
BETZtechnik 20:8aebc19d9d1d 287 }
BETZtechnik 20:8aebc19d9d1d 288
BETZtechnik 20:8aebc19d9d1d 289 else if (c=='2') {
BETZtechnik 20:8aebc19d9d1d 290 Ones = 2;
BETZtechnik 20:8aebc19d9d1d 291 }
BETZtechnik 20:8aebc19d9d1d 292
BETZtechnik 20:8aebc19d9d1d 293 else if (c=='3') {
BETZtechnik 20:8aebc19d9d1d 294 Ones = 3;
BETZtechnik 20:8aebc19d9d1d 295 }
BETZtechnik 20:8aebc19d9d1d 296
BETZtechnik 20:8aebc19d9d1d 297 else if (c=='4') {
BETZtechnik 20:8aebc19d9d1d 298 Ones = 4;
BETZtechnik 20:8aebc19d9d1d 299 }
BETZtechnik 20:8aebc19d9d1d 300
BETZtechnik 20:8aebc19d9d1d 301 else if (c=='5') {
BETZtechnik 20:8aebc19d9d1d 302 Ones = 5;
BETZtechnik 20:8aebc19d9d1d 303 }
BETZtechnik 20:8aebc19d9d1d 304
BETZtechnik 20:8aebc19d9d1d 305 else if (c=='6') {
BETZtechnik 20:8aebc19d9d1d 306 Ones = 6;
BETZtechnik 20:8aebc19d9d1d 307 }
BETZtechnik 20:8aebc19d9d1d 308
BETZtechnik 20:8aebc19d9d1d 309 else if (c=='7') {
BETZtechnik 20:8aebc19d9d1d 310 Ones = 7;
BETZtechnik 20:8aebc19d9d1d 311 }
BETZtechnik 20:8aebc19d9d1d 312
BETZtechnik 20:8aebc19d9d1d 313 else if (c=='8') {
BETZtechnik 20:8aebc19d9d1d 314 Ones = 8;
BETZtechnik 20:8aebc19d9d1d 315 }
BETZtechnik 20:8aebc19d9d1d 316
BETZtechnik 20:8aebc19d9d1d 317 else if (c=='9') {
BETZtechnik 20:8aebc19d9d1d 318 Ones = 9;
BETZtechnik 20:8aebc19d9d1d 319 }
BETZtechnik 20:8aebc19d9d1d 320
BETZtechnik 21:db507362ca7a 321
BETZtechnik 21:db507362ca7a 322
BETZtechnik 21:db507362ca7a 323 if (currentNozzle == 1){
BETZtechnik 21:db507362ca7a 324 n1Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
BETZtechnik 21:db507362ca7a 325 twitchStart = 0; // trigger sweep from new angle
BETZtechnik 21:db507362ca7a 326 servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 327 //servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 328 index=0;
BETZtechnik 21:db507362ca7a 329 }
BETZtechnik 21:db507362ca7a 330
BETZtechnik 21:db507362ca7a 331
BETZtechnik 21:db507362ca7a 332
BETZtechnik 21:db507362ca7a 333
BETZtechnik 20:8aebc19d9d1d 334 }
BETZtechnik 20:8aebc19d9d1d 335 }
BETZtechnik 21:db507362ca7a 336
BETZtechnik 21:db507362ca7a 337
BETZtechnik 21:db507362ca7a 338
BETZtechnik 21:db507362ca7a 339 if (twitch == 0) {
BETZtechnik 21:db507362ca7a 340 if (servo1Pos != lastServo1Pos) {
BETZtechnik 21:db507362ca7a 341 twitchStart = 0;
BETZtechnik 21:db507362ca7a 342 twitchDirection = 0;
BETZtechnik 21:db507362ca7a 343 servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 344 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 345 //servo2Pos = servo2Scale.from(n2Pos);
BETZtechnik 21:db507362ca7a 346 //servo2.pulsewidth_us(servo2Pos);
BETZtechnik 21:db507362ca7a 347 lastServo1Pos = servo1Pos;
BETZtechnik 21:db507362ca7a 348 }
BETZtechnik 21:db507362ca7a 349 }
BETZtechnik 21:db507362ca7a 350
BETZtechnik 21:db507362ca7a 351 if (twitch > 0) {
BETZtechnik 21:db507362ca7a 352 if (twitchStart == 0) {
BETZtechnik 21:db507362ca7a 353 //tw.start(); // start timer for nozzle sweep
BETZtechnik 21:db507362ca7a 354 //servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 355 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 356 lastTwitchMove = t.read_ms(); // store time of last nozzle movement
BETZtechnik 21:db507362ca7a 357 minTwitch = servo1Pos; // store original servo postion for bottom of sweep
BETZtechnik 21:db507362ca7a 358 if (twitch == 1) {
BETZtechnik 21:db507362ca7a 359 maxTwitch = servo1Pos + 150; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
BETZtechnik 21:db507362ca7a 360 }
BETZtechnik 21:db507362ca7a 361 if (twitch == 2) {
BETZtechnik 21:db507362ca7a 362 maxTwitch = servo1Pos + 200;
BETZtechnik 21:db507362ca7a 363 }
BETZtechnik 21:db507362ca7a 364 if (twitch == 3) {
BETZtechnik 21:db507362ca7a 365 maxTwitch = servo1Pos + 250;
BETZtechnik 21:db507362ca7a 366 }
BETZtechnik 21:db507362ca7a 367 twitchStart = 1;
BETZtechnik 21:db507362ca7a 368
BETZtechnik 21:db507362ca7a 369 }
BETZtechnik 21:db507362ca7a 370 if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
BETZtechnik 21:db507362ca7a 371 if (twitchDirection == 0) { //going up
BETZtechnik 21:db507362ca7a 372 servo1Pos = servo1Pos + 25; // add variable amount of uS to the servo signal
BETZtechnik 21:db507362ca7a 373
BETZtechnik 21:db507362ca7a 374 if (servo1Pos > maxServo) {
BETZtechnik 21:db507362ca7a 375 servo1Pos = maxServo;
BETZtechnik 21:db507362ca7a 376 twitchDirection = 1; //reverse direction
BETZtechnik 21:db507362ca7a 377 }
BETZtechnik 21:db507362ca7a 378 if (servo1Pos > maxTwitch) {
BETZtechnik 21:db507362ca7a 379 servo1Pos = maxTwitch;
BETZtechnik 21:db507362ca7a 380 twitchDirection = 1; //reverse direction
BETZtechnik 21:db507362ca7a 381 }
BETZtechnik 21:db507362ca7a 382 lastTwitchMove = t.read_ms();
BETZtechnik 21:db507362ca7a 383 }
BETZtechnik 21:db507362ca7a 384
BETZtechnik 21:db507362ca7a 385 if (twitchDirection == 1) { // going down
BETZtechnik 21:db507362ca7a 386 servo1Pos = servo1Pos - 25; // add variable amount of Ms to the servo signal
BETZtechnik 21:db507362ca7a 387 if (servo1Pos < minServo) {
BETZtechnik 21:db507362ca7a 388 servo1Pos = minServo;
BETZtechnik 21:db507362ca7a 389 twitchDirection = 0; //reverse direction
BETZtechnik 21:db507362ca7a 390 }
BETZtechnik 21:db507362ca7a 391 if (servo1Pos < minTwitch) {
BETZtechnik 21:db507362ca7a 392 servo1Pos = minTwitch;
BETZtechnik 21:db507362ca7a 393 twitchDirection = 0; //reverse direction
BETZtechnik 21:db507362ca7a 394 }
BETZtechnik 21:db507362ca7a 395 lastTwitchMove = t.read_ms();
BETZtechnik 21:db507362ca7a 396 }
BETZtechnik 21:db507362ca7a 397 if (servo1Pos != lastServo1Pos) {
BETZtechnik 21:db507362ca7a 398 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 399 lastServo1Pos = servo1Pos;
BETZtechnik 21:db507362ca7a 400 }
BETZtechnik 21:db507362ca7a 401 }
BETZtechnik 21:db507362ca7a 402
BETZtechnik 20:8aebc19d9d1d 403
BETZtechnik 20:8aebc19d9d1d 404
BETZtechnik 20:8aebc19d9d1d 405
BETZtechnik 21:db507362ca7a 406 }
BETZtechnik 0:09419d572e90 407
BETZtechnik 0:09419d572e90 408
BETZtechnik 8:6bb07a893f53 409 //********************************* Inputs serial *******************************
BETZtechnik 8:6bb07a893f53 410
BETZtechnik 8:6bb07a893f53 411
BETZtechnik 8:6bb07a893f53 412 while (inputs.readable())
BETZtechnik 8:6bb07a893f53 413 {
BETZtechnik 8:6bb07a893f53 414
BETZtechnik 8:6bb07a893f53 415
BETZtechnik 8:6bb07a893f53 416 c = inputs.getc();
BETZtechnik 8:6bb07a893f53 417
BETZtechnik 8:6bb07a893f53 418 if (c == 'T'){
BETZtechnik 8:6bb07a893f53 419
BETZtechnik 8:6bb07a893f53 420 index = 1;
BETZtechnik 8:6bb07a893f53 421 servo2=1;
BETZtechnik 8:6bb07a893f53 422 }
BETZtechnik 8:6bb07a893f53 423
BETZtechnik 8:6bb07a893f53 424
BETZtechnik 8:6bb07a893f53 425 if (index == 1){
BETZtechnik 8:6bb07a893f53 426 c = inputs.getc();
BETZtechnik 8:6bb07a893f53 427
BETZtechnik 8:6bb07a893f53 428
BETZtechnik 8:6bb07a893f53 429 if (c=='0'){
BETZtechnik 8:6bb07a893f53 430 Tens = 0;
BETZtechnik 8:6bb07a893f53 431 servo2=1;
BETZtechnik 8:6bb07a893f53 432 }
BETZtechnik 8:6bb07a893f53 433
BETZtechnik 8:6bb07a893f53 434 else if (c=='1'){
BETZtechnik 8:6bb07a893f53 435 Tens = 1;
BETZtechnik 8:6bb07a893f53 436
BETZtechnik 8:6bb07a893f53 437
BETZtechnik 8:6bb07a893f53 438 }
BETZtechnik 8:6bb07a893f53 439
BETZtechnik 8:6bb07a893f53 440 else if (c=='2'){
BETZtechnik 8:6bb07a893f53 441 Tens = 2;
BETZtechnik 8:6bb07a893f53 442
BETZtechnik 8:6bb07a893f53 443 }
BETZtechnik 8:6bb07a893f53 444
BETZtechnik 8:6bb07a893f53 445 else if (c=='3'){
BETZtechnik 8:6bb07a893f53 446 Tens = 3;
BETZtechnik 8:6bb07a893f53 447
BETZtechnik 8:6bb07a893f53 448 }
BETZtechnik 8:6bb07a893f53 449
BETZtechnik 8:6bb07a893f53 450 else if (c=='4'){
BETZtechnik 8:6bb07a893f53 451 Tens = 4;
BETZtechnik 8:6bb07a893f53 452 }
BETZtechnik 8:6bb07a893f53 453
BETZtechnik 8:6bb07a893f53 454 else if (c=='5'){
BETZtechnik 8:6bb07a893f53 455 Tens = 5;
BETZtechnik 8:6bb07a893f53 456 }
BETZtechnik 8:6bb07a893f53 457
BETZtechnik 8:6bb07a893f53 458 else if (c=='6'){
BETZtechnik 8:6bb07a893f53 459 Tens = 6;
BETZtechnik 8:6bb07a893f53 460 }
BETZtechnik 8:6bb07a893f53 461
BETZtechnik 8:6bb07a893f53 462 else if (c=='7'){
BETZtechnik 8:6bb07a893f53 463 Tens = 7;
BETZtechnik 8:6bb07a893f53 464 }
BETZtechnik 8:6bb07a893f53 465
BETZtechnik 8:6bb07a893f53 466 else if (c=='8'){
BETZtechnik 8:6bb07a893f53 467 Tens = 8;
BETZtechnik 8:6bb07a893f53 468 }
BETZtechnik 8:6bb07a893f53 469
BETZtechnik 8:6bb07a893f53 470 else if (c=='9'){
BETZtechnik 8:6bb07a893f53 471 Tens = 9;
BETZtechnik 8:6bb07a893f53 472 }
BETZtechnik 8:6bb07a893f53 473
BETZtechnik 8:6bb07a893f53 474 else if (c=='0'){
BETZtechnik 8:6bb07a893f53 475 Tens = 0;
BETZtechnik 8:6bb07a893f53 476 }
BETZtechnik 8:6bb07a893f53 477 index = 2;
BETZtechnik 8:6bb07a893f53 478 }
BETZtechnik 8:6bb07a893f53 479
BETZtechnik 8:6bb07a893f53 480
BETZtechnik 8:6bb07a893f53 481 if (index == 2){
BETZtechnik 8:6bb07a893f53 482 c = inputs.getc();
BETZtechnik 8:6bb07a893f53 483 if (c=='0'){
BETZtechnik 8:6bb07a893f53 484 Ones = 0;
BETZtechnik 8:6bb07a893f53 485
BETZtechnik 8:6bb07a893f53 486 }
BETZtechnik 8:6bb07a893f53 487
BETZtechnik 8:6bb07a893f53 488 else if (c=='1'){
BETZtechnik 8:6bb07a893f53 489 Ones = 1;
BETZtechnik 8:6bb07a893f53 490
BETZtechnik 8:6bb07a893f53 491
BETZtechnik 8:6bb07a893f53 492 }
BETZtechnik 8:6bb07a893f53 493
BETZtechnik 8:6bb07a893f53 494 else if (c=='2'){
BETZtechnik 8:6bb07a893f53 495 Ones = 2;
BETZtechnik 8:6bb07a893f53 496
BETZtechnik 8:6bb07a893f53 497 }
BETZtechnik 8:6bb07a893f53 498
BETZtechnik 8:6bb07a893f53 499 else if (c=='3'){
BETZtechnik 8:6bb07a893f53 500 Ones = 3;
BETZtechnik 8:6bb07a893f53 501
BETZtechnik 8:6bb07a893f53 502 }
BETZtechnik 8:6bb07a893f53 503
BETZtechnik 8:6bb07a893f53 504 else if (c=='4'){
BETZtechnik 8:6bb07a893f53 505 Ones = 4;
BETZtechnik 8:6bb07a893f53 506 }
BETZtechnik 8:6bb07a893f53 507
BETZtechnik 8:6bb07a893f53 508 else if (c=='5'){
BETZtechnik 8:6bb07a893f53 509 Ones = 5;
BETZtechnik 8:6bb07a893f53 510 }
BETZtechnik 8:6bb07a893f53 511
BETZtechnik 8:6bb07a893f53 512 else if (c=='6'){
BETZtechnik 8:6bb07a893f53 513 Ones = 6;
BETZtechnik 8:6bb07a893f53 514 }
BETZtechnik 8:6bb07a893f53 515
BETZtechnik 8:6bb07a893f53 516 else if (c=='7'){
BETZtechnik 8:6bb07a893f53 517 Ones = 7;
BETZtechnik 8:6bb07a893f53 518 }
BETZtechnik 8:6bb07a893f53 519
BETZtechnik 8:6bb07a893f53 520 else if (c=='8'){
BETZtechnik 8:6bb07a893f53 521 Ones = 8;
BETZtechnik 8:6bb07a893f53 522 }
BETZtechnik 8:6bb07a893f53 523
BETZtechnik 8:6bb07a893f53 524 else if (c=='9'){
BETZtechnik 8:6bb07a893f53 525 Ones = 9;
BETZtechnik 8:6bb07a893f53 526 }
BETZtechnik 8:6bb07a893f53 527
BETZtechnik 8:6bb07a893f53 528 else if (c=='0'){
BETZtechnik 8:6bb07a893f53 529 Ones = 0;
BETZtechnik 8:6bb07a893f53 530 }
BETZtechnik 8:6bb07a893f53 531 int tempToolNo = (Tens*10)+100+Ones;
BETZtechnik 8:6bb07a893f53 532 dir=1;
BETZtechnik 8:6bb07a893f53 533 display.printf("T");
BETZtechnik 8:6bb07a893f53 534 wait(0.05);
BETZtechnik 8:6bb07a893f53 535 display.printf("%d", tempToolNo);
BETZtechnik 8:6bb07a893f53 536 wait(0.001);
BETZtechnik 8:6bb07a893f53 537 dir=0;
BETZtechnik 8:6bb07a893f53 538 index = 0;
BETZtechnik 8:6bb07a893f53 539 tempToolNo=0;
BETZtechnik 8:6bb07a893f53 540
BETZtechnik 8:6bb07a893f53 541 }
BETZtechnik 8:6bb07a893f53 542
BETZtechnik 8:6bb07a893f53 543
BETZtechnik 8:6bb07a893f53 544 }
BETZtechnik 8:6bb07a893f53 545
BETZtechnik 8:6bb07a893f53 546
BETZtechnik 8:6bb07a893f53 547 // *****************************************************************************
BETZtechnik 5:18c6ff1370d0 548
BETZtechnik 0:09419d572e90 549
BETZtechnik 2:79c03ac27b72 550
BETZtechnik 2:79c03ac27b72 551 }
BETZtechnik 2:79c03ac27b72 552 }