Mark and Peter / Mbed 2 deprecated 11u68Displaytest

Remote inc coolant nozzle control program

Dependencies:   mbed reScale USBDevice

Dependents:   Nozzle_inputs Nozzle_display

main.cpp@22:a3ada529b264, 2019-04-11 (annotated)

Committer:
BETZtechnik
Date:
Thu Apr 11 23:22:45 2019 +0000
Revision:
22:a3ada529b264
Parent:
21:db507362ca7a
Child:
23:baedb9e32b6d
Child:
24:55fafbb82a37
everything including twitch appears to be working, no freezing 2019-04-11;

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 22:a3ada529b264 28 reScale servo1Scale(90,-90,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 22:a3ada529b264 45 int tReadMs = 10;
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 22:a3ada529b264 192 }
BETZtechnik 20:8aebc19d9d1d 193
BETZtechnik 22:a3ada529b264 194 else if (c == '2') { //N2
BETZtechnik 20:8aebc19d9d1d 195 currentNozzle = 1;
BETZtechnik 20:8aebc19d9d1d 196 Hundreds = 1;
BETZtechnik 20:8aebc19d9d1d 197 index = 3;
BETZtechnik 20:8aebc19d9d1d 198 }
BETZtechnik 20:8aebc19d9d1d 199
BETZtechnik 20:8aebc19d9d1d 200 if (c == '3') { //N2 // Display adding 390 for N2 so 300 to 480 would be -90 to +90
BETZtechnik 20:8aebc19d9d1d 201 currentNozzle = 2;
BETZtechnik 20:8aebc19d9d1d 202 Hundreds = 0;
BETZtechnik 20:8aebc19d9d1d 203 index = 3;
BETZtechnik 20:8aebc19d9d1d 204 }
BETZtechnik 20:8aebc19d9d1d 205
BETZtechnik 20:8aebc19d9d1d 206 if (c == '4') { //N2
BETZtechnik 20:8aebc19d9d1d 207 currentNozzle = 2;
BETZtechnik 20:8aebc19d9d1d 208 Hundreds = 1;
BETZtechnik 20:8aebc19d9d1d 209 index = 3;
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
BETZtechnik 20:8aebc19d9d1d 217 if (index == 3) {
BETZtechnik 20:8aebc19d9d1d 218
BETZtechnik 20:8aebc19d9d1d 219 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 220
BETZtechnik 20:8aebc19d9d1d 221 if (c=='0') {
BETZtechnik 20:8aebc19d9d1d 222 Tens = 0;
BETZtechnik 20:8aebc19d9d1d 223 index = 4;
BETZtechnik 20:8aebc19d9d1d 224
BETZtechnik 20:8aebc19d9d1d 225 }
BETZtechnik 20:8aebc19d9d1d 226
BETZtechnik 20:8aebc19d9d1d 227 else if (c=='1') {
BETZtechnik 20:8aebc19d9d1d 228 Tens = 1;
BETZtechnik 20:8aebc19d9d1d 229 index = 4;
BETZtechnik 20:8aebc19d9d1d 230
BETZtechnik 20:8aebc19d9d1d 231 }
BETZtechnik 20:8aebc19d9d1d 232
BETZtechnik 20:8aebc19d9d1d 233 else if (c=='2') {
BETZtechnik 20:8aebc19d9d1d 234 Tens = 2;
BETZtechnik 20:8aebc19d9d1d 235 index = 4;
BETZtechnik 20:8aebc19d9d1d 236
BETZtechnik 20:8aebc19d9d1d 237 }
BETZtechnik 20:8aebc19d9d1d 238
BETZtechnik 20:8aebc19d9d1d 239 else if (c=='3') {
BETZtechnik 20:8aebc19d9d1d 240 Tens = 3;
BETZtechnik 20:8aebc19d9d1d 241 index = 4;
BETZtechnik 20:8aebc19d9d1d 242
BETZtechnik 20:8aebc19d9d1d 243 }
BETZtechnik 20:8aebc19d9d1d 244
BETZtechnik 20:8aebc19d9d1d 245 else if (c=='4') {
BETZtechnik 20:8aebc19d9d1d 246 Tens = 4;
BETZtechnik 20:8aebc19d9d1d 247 index = 4;
BETZtechnik 20:8aebc19d9d1d 248 }
BETZtechnik 20:8aebc19d9d1d 249
BETZtechnik 20:8aebc19d9d1d 250 else if (c=='5') {
BETZtechnik 20:8aebc19d9d1d 251 Tens = 5;
BETZtechnik 20:8aebc19d9d1d 252 index = 4;
BETZtechnik 20:8aebc19d9d1d 253 }
BETZtechnik 20:8aebc19d9d1d 254
BETZtechnik 20:8aebc19d9d1d 255 else if (c=='6') {
BETZtechnik 20:8aebc19d9d1d 256 Tens = 6;
BETZtechnik 20:8aebc19d9d1d 257 index = 4;
BETZtechnik 20:8aebc19d9d1d 258 }
BETZtechnik 20:8aebc19d9d1d 259
BETZtechnik 20:8aebc19d9d1d 260 else if (c=='7') {
BETZtechnik 20:8aebc19d9d1d 261 Tens = 7;
BETZtechnik 20:8aebc19d9d1d 262 index = 4;
BETZtechnik 20:8aebc19d9d1d 263 }
BETZtechnik 20:8aebc19d9d1d 264
BETZtechnik 20:8aebc19d9d1d 265 else if (c=='8') {
BETZtechnik 20:8aebc19d9d1d 266 Tens = 8;
BETZtechnik 20:8aebc19d9d1d 267 index = 4;
BETZtechnik 20:8aebc19d9d1d 268 }
BETZtechnik 20:8aebc19d9d1d 269
BETZtechnik 20:8aebc19d9d1d 270 else if (c=='9') {
BETZtechnik 20:8aebc19d9d1d 271 Tens = 9;
BETZtechnik 20:8aebc19d9d1d 272 index = 4;
BETZtechnik 20:8aebc19d9d1d 273 }
BETZtechnik 20:8aebc19d9d1d 274
BETZtechnik 20:8aebc19d9d1d 275
BETZtechnik 20:8aebc19d9d1d 276 }
BETZtechnik 20:8aebc19d9d1d 277
BETZtechnik 20:8aebc19d9d1d 278 if (index == 4) {
BETZtechnik 20:8aebc19d9d1d 279
BETZtechnik 20:8aebc19d9d1d 280 c = display.getc();
BETZtechnik 20:8aebc19d9d1d 281
BETZtechnik 20:8aebc19d9d1d 282 if (c=='0') {
BETZtechnik 20:8aebc19d9d1d 283 Ones = 0;
BETZtechnik 20:8aebc19d9d1d 284 }
BETZtechnik 20:8aebc19d9d1d 285
BETZtechnik 20:8aebc19d9d1d 286 else if (c=='1') {
BETZtechnik 20:8aebc19d9d1d 287 Ones = 1;
BETZtechnik 20:8aebc19d9d1d 288 }
BETZtechnik 20:8aebc19d9d1d 289
BETZtechnik 20:8aebc19d9d1d 290 else if (c=='2') {
BETZtechnik 20:8aebc19d9d1d 291 Ones = 2;
BETZtechnik 20:8aebc19d9d1d 292 }
BETZtechnik 20:8aebc19d9d1d 293
BETZtechnik 20:8aebc19d9d1d 294 else if (c=='3') {
BETZtechnik 20:8aebc19d9d1d 295 Ones = 3;
BETZtechnik 20:8aebc19d9d1d 296 }
BETZtechnik 20:8aebc19d9d1d 297
BETZtechnik 20:8aebc19d9d1d 298 else if (c=='4') {
BETZtechnik 20:8aebc19d9d1d 299 Ones = 4;
BETZtechnik 20:8aebc19d9d1d 300 }
BETZtechnik 20:8aebc19d9d1d 301
BETZtechnik 20:8aebc19d9d1d 302 else if (c=='5') {
BETZtechnik 20:8aebc19d9d1d 303 Ones = 5;
BETZtechnik 20:8aebc19d9d1d 304 }
BETZtechnik 20:8aebc19d9d1d 305
BETZtechnik 20:8aebc19d9d1d 306 else if (c=='6') {
BETZtechnik 20:8aebc19d9d1d 307 Ones = 6;
BETZtechnik 20:8aebc19d9d1d 308 }
BETZtechnik 20:8aebc19d9d1d 309
BETZtechnik 20:8aebc19d9d1d 310 else if (c=='7') {
BETZtechnik 20:8aebc19d9d1d 311 Ones = 7;
BETZtechnik 20:8aebc19d9d1d 312 }
BETZtechnik 20:8aebc19d9d1d 313
BETZtechnik 20:8aebc19d9d1d 314 else if (c=='8') {
BETZtechnik 20:8aebc19d9d1d 315 Ones = 8;
BETZtechnik 20:8aebc19d9d1d 316 }
BETZtechnik 20:8aebc19d9d1d 317
BETZtechnik 20:8aebc19d9d1d 318 else if (c=='9') {
BETZtechnik 20:8aebc19d9d1d 319 Ones = 9;
BETZtechnik 20:8aebc19d9d1d 320 }
BETZtechnik 20:8aebc19d9d1d 321
BETZtechnik 21:db507362ca7a 322
BETZtechnik 21:db507362ca7a 323
BETZtechnik 21:db507362ca7a 324 if (currentNozzle == 1){
BETZtechnik 21:db507362ca7a 325 n1Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
BETZtechnik 21:db507362ca7a 326 twitchStart = 0; // trigger sweep from new angle
BETZtechnik 21:db507362ca7a 327 servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 328 //servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 329 index=0;
BETZtechnik 21:db507362ca7a 330 }
BETZtechnik 21:db507362ca7a 331
BETZtechnik 21:db507362ca7a 332
BETZtechnik 21:db507362ca7a 333
BETZtechnik 21:db507362ca7a 334
BETZtechnik 20:8aebc19d9d1d 335 }
BETZtechnik 20:8aebc19d9d1d 336 }
BETZtechnik 21:db507362ca7a 337
BETZtechnik 21:db507362ca7a 338
BETZtechnik 21:db507362ca7a 339
BETZtechnik 21:db507362ca7a 340 if (twitch == 0) {
BETZtechnik 21:db507362ca7a 341 if (servo1Pos != lastServo1Pos) {
BETZtechnik 21:db507362ca7a 342 twitchStart = 0;
BETZtechnik 21:db507362ca7a 343 twitchDirection = 0;
BETZtechnik 21:db507362ca7a 344 servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 345 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 346 //servo2Pos = servo2Scale.from(n2Pos);
BETZtechnik 21:db507362ca7a 347 //servo2.pulsewidth_us(servo2Pos);
BETZtechnik 21:db507362ca7a 348 lastServo1Pos = servo1Pos;
BETZtechnik 21:db507362ca7a 349 }
BETZtechnik 21:db507362ca7a 350 }
BETZtechnik 21:db507362ca7a 351
BETZtechnik 21:db507362ca7a 352 if (twitch > 0) {
BETZtechnik 21:db507362ca7a 353 if (twitchStart == 0) {
BETZtechnik 21:db507362ca7a 354 //tw.start(); // start timer for nozzle sweep
BETZtechnik 21:db507362ca7a 355 //servo1Pos = servo1Scale.from(n1Pos);
BETZtechnik 21:db507362ca7a 356 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 357 lastTwitchMove = t.read_ms(); // store time of last nozzle movement
BETZtechnik 21:db507362ca7a 358 minTwitch = servo1Pos; // store original servo postion for bottom of sweep
BETZtechnik 21:db507362ca7a 359 if (twitch == 1) {
BETZtechnik 22:a3ada529b264 360 maxTwitch = servo1Pos - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
BETZtechnik 21:db507362ca7a 361 }
BETZtechnik 21:db507362ca7a 362 if (twitch == 2) {
BETZtechnik 22:a3ada529b264 363 maxTwitch = servo1Pos - 150;
BETZtechnik 21:db507362ca7a 364 }
BETZtechnik 21:db507362ca7a 365 if (twitch == 3) {
BETZtechnik 22:a3ada529b264 366 maxTwitch = servo1Pos - 200;
BETZtechnik 21:db507362ca7a 367 }
BETZtechnik 21:db507362ca7a 368 twitchStart = 1;
BETZtechnik 21:db507362ca7a 369
BETZtechnik 21:db507362ca7a 370 }
BETZtechnik 21:db507362ca7a 371 if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
BETZtechnik 21:db507362ca7a 372 if (twitchDirection == 0) { //going up
BETZtechnik 22:a3ada529b264 373 servo1Pos = servo1Pos - 10; // add variable amount of uS to the servo signal
BETZtechnik 21:db507362ca7a 374
BETZtechnik 22:a3ada529b264 375 if (servo1Pos < minServo) {
BETZtechnik 22:a3ada529b264 376 servo1Pos = minServo;
BETZtechnik 21:db507362ca7a 377 twitchDirection = 1; //reverse direction
BETZtechnik 21:db507362ca7a 378 }
BETZtechnik 22:a3ada529b264 379 if (servo1Pos < maxTwitch) {
BETZtechnik 21:db507362ca7a 380 servo1Pos = maxTwitch;
BETZtechnik 21:db507362ca7a 381 twitchDirection = 1; //reverse direction
BETZtechnik 21:db507362ca7a 382 }
BETZtechnik 21:db507362ca7a 383 lastTwitchMove = t.read_ms();
BETZtechnik 21:db507362ca7a 384 }
BETZtechnik 21:db507362ca7a 385
BETZtechnik 21:db507362ca7a 386 if (twitchDirection == 1) { // going down
BETZtechnik 22:a3ada529b264 387 servo1Pos = servo1Pos + 10; // add variable amount of Ms to the servo signal
BETZtechnik 22:a3ada529b264 388 if (servo1Pos > maxServo) {
BETZtechnik 22:a3ada529b264 389 servo1Pos = maxServo;
BETZtechnik 21:db507362ca7a 390 twitchDirection = 0; //reverse direction
BETZtechnik 21:db507362ca7a 391 }
BETZtechnik 22:a3ada529b264 392 if (servo1Pos > minTwitch) {
BETZtechnik 21:db507362ca7a 393 servo1Pos = minTwitch;
BETZtechnik 21:db507362ca7a 394 twitchDirection = 0; //reverse direction
BETZtechnik 21:db507362ca7a 395 }
BETZtechnik 21:db507362ca7a 396 lastTwitchMove = t.read_ms();
BETZtechnik 21:db507362ca7a 397 }
BETZtechnik 21:db507362ca7a 398 if (servo1Pos != lastServo1Pos) {
BETZtechnik 21:db507362ca7a 399 servo1.pulsewidth_us(servo1Pos);
BETZtechnik 21:db507362ca7a 400 lastServo1Pos = servo1Pos;
BETZtechnik 21:db507362ca7a 401 }
BETZtechnik 21:db507362ca7a 402 }
BETZtechnik 21:db507362ca7a 403
BETZtechnik 20:8aebc19d9d1d 404
BETZtechnik 20:8aebc19d9d1d 405
BETZtechnik 20:8aebc19d9d1d 406
BETZtechnik 21:db507362ca7a 407 }
BETZtechnik 0:09419d572e90 408
BETZtechnik 0:09419d572e90 409
BETZtechnik 8:6bb07a893f53 410 //********************************* Inputs serial *******************************
BETZtechnik 8:6bb07a893f53 411
BETZtechnik 8:6bb07a893f53 412
BETZtechnik 8:6bb07a893f53 413 while (inputs.readable())
BETZtechnik 8:6bb07a893f53 414 {
BETZtechnik 8:6bb07a893f53 415
BETZtechnik 8:6bb07a893f53 416
BETZtechnik 8:6bb07a893f53 417 c = inputs.getc();
BETZtechnik 8:6bb07a893f53 418
BETZtechnik 8:6bb07a893f53 419 if (c == 'T'){
BETZtechnik 8:6bb07a893f53 420
BETZtechnik 8:6bb07a893f53 421 index = 1;
BETZtechnik 8:6bb07a893f53 422 servo2=1;
BETZtechnik 8:6bb07a893f53 423 }
BETZtechnik 8:6bb07a893f53 424
BETZtechnik 8:6bb07a893f53 425
BETZtechnik 8:6bb07a893f53 426 if (index == 1){
BETZtechnik 8:6bb07a893f53 427 c = inputs.getc();
BETZtechnik 8:6bb07a893f53 428
BETZtechnik 8:6bb07a893f53 429
BETZtechnik 8:6bb07a893f53 430 if (c=='0'){
BETZtechnik 8:6bb07a893f53 431 Tens = 0;
BETZtechnik 8:6bb07a893f53 432 servo2=1;
BETZtechnik 8:6bb07a893f53 433 }
BETZtechnik 8:6bb07a893f53 434
BETZtechnik 8:6bb07a893f53 435 else if (c=='1'){
BETZtechnik 8:6bb07a893f53 436 Tens = 1;
BETZtechnik 8:6bb07a893f53 437
BETZtechnik 8:6bb07a893f53 438
BETZtechnik 8:6bb07a893f53 439 }
BETZtechnik 8:6bb07a893f53 440
BETZtechnik 8:6bb07a893f53 441 else if (c=='2'){
BETZtechnik 8:6bb07a893f53 442 Tens = 2;
BETZtechnik 8:6bb07a893f53 443
BETZtechnik 8:6bb07a893f53 444 }
BETZtechnik 8:6bb07a893f53 445
BETZtechnik 8:6bb07a893f53 446 else if (c=='3'){
BETZtechnik 8:6bb07a893f53 447 Tens = 3;
BETZtechnik 8:6bb07a893f53 448
BETZtechnik 8:6bb07a893f53 449 }
BETZtechnik 8:6bb07a893f53 450
BETZtechnik 8:6bb07a893f53 451 else if (c=='4'){
BETZtechnik 8:6bb07a893f53 452 Tens = 4;
BETZtechnik 8:6bb07a893f53 453 }
BETZtechnik 8:6bb07a893f53 454
BETZtechnik 8:6bb07a893f53 455 else if (c=='5'){
BETZtechnik 8:6bb07a893f53 456 Tens = 5;
BETZtechnik 8:6bb07a893f53 457 }
BETZtechnik 8:6bb07a893f53 458
BETZtechnik 8:6bb07a893f53 459 else if (c=='6'){
BETZtechnik 8:6bb07a893f53 460 Tens = 6;
BETZtechnik 8:6bb07a893f53 461 }
BETZtechnik 8:6bb07a893f53 462
BETZtechnik 8:6bb07a893f53 463 else if (c=='7'){
BETZtechnik 8:6bb07a893f53 464 Tens = 7;
BETZtechnik 8:6bb07a893f53 465 }
BETZtechnik 8:6bb07a893f53 466
BETZtechnik 8:6bb07a893f53 467 else if (c=='8'){
BETZtechnik 8:6bb07a893f53 468 Tens = 8;
BETZtechnik 8:6bb07a893f53 469 }
BETZtechnik 8:6bb07a893f53 470
BETZtechnik 8:6bb07a893f53 471 else if (c=='9'){
BETZtechnik 8:6bb07a893f53 472 Tens = 9;
BETZtechnik 8:6bb07a893f53 473 }
BETZtechnik 8:6bb07a893f53 474
BETZtechnik 8:6bb07a893f53 475 else if (c=='0'){
BETZtechnik 8:6bb07a893f53 476 Tens = 0;
BETZtechnik 8:6bb07a893f53 477 }
BETZtechnik 8:6bb07a893f53 478 index = 2;
BETZtechnik 8:6bb07a893f53 479 }
BETZtechnik 8:6bb07a893f53 480
BETZtechnik 8:6bb07a893f53 481
BETZtechnik 8:6bb07a893f53 482 if (index == 2){
BETZtechnik 8:6bb07a893f53 483 c = inputs.getc();
BETZtechnik 8:6bb07a893f53 484 if (c=='0'){
BETZtechnik 8:6bb07a893f53 485 Ones = 0;
BETZtechnik 8:6bb07a893f53 486
BETZtechnik 8:6bb07a893f53 487 }
BETZtechnik 8:6bb07a893f53 488
BETZtechnik 8:6bb07a893f53 489 else if (c=='1'){
BETZtechnik 8:6bb07a893f53 490 Ones = 1;
BETZtechnik 8:6bb07a893f53 491
BETZtechnik 8:6bb07a893f53 492
BETZtechnik 8:6bb07a893f53 493 }
BETZtechnik 8:6bb07a893f53 494
BETZtechnik 8:6bb07a893f53 495 else if (c=='2'){
BETZtechnik 8:6bb07a893f53 496 Ones = 2;
BETZtechnik 8:6bb07a893f53 497
BETZtechnik 8:6bb07a893f53 498 }
BETZtechnik 8:6bb07a893f53 499
BETZtechnik 8:6bb07a893f53 500 else if (c=='3'){
BETZtechnik 8:6bb07a893f53 501 Ones = 3;
BETZtechnik 8:6bb07a893f53 502
BETZtechnik 8:6bb07a893f53 503 }
BETZtechnik 8:6bb07a893f53 504
BETZtechnik 8:6bb07a893f53 505 else if (c=='4'){
BETZtechnik 8:6bb07a893f53 506 Ones = 4;
BETZtechnik 8:6bb07a893f53 507 }
BETZtechnik 8:6bb07a893f53 508
BETZtechnik 8:6bb07a893f53 509 else if (c=='5'){
BETZtechnik 8:6bb07a893f53 510 Ones = 5;
BETZtechnik 8:6bb07a893f53 511 }
BETZtechnik 8:6bb07a893f53 512
BETZtechnik 8:6bb07a893f53 513 else if (c=='6'){
BETZtechnik 8:6bb07a893f53 514 Ones = 6;
BETZtechnik 8:6bb07a893f53 515 }
BETZtechnik 8:6bb07a893f53 516
BETZtechnik 8:6bb07a893f53 517 else if (c=='7'){
BETZtechnik 8:6bb07a893f53 518 Ones = 7;
BETZtechnik 8:6bb07a893f53 519 }
BETZtechnik 8:6bb07a893f53 520
BETZtechnik 8:6bb07a893f53 521 else if (c=='8'){
BETZtechnik 8:6bb07a893f53 522 Ones = 8;
BETZtechnik 8:6bb07a893f53 523 }
BETZtechnik 8:6bb07a893f53 524
BETZtechnik 8:6bb07a893f53 525 else if (c=='9'){
BETZtechnik 8:6bb07a893f53 526 Ones = 9;
BETZtechnik 8:6bb07a893f53 527 }
BETZtechnik 8:6bb07a893f53 528
BETZtechnik 8:6bb07a893f53 529 else if (c=='0'){
BETZtechnik 8:6bb07a893f53 530 Ones = 0;
BETZtechnik 8:6bb07a893f53 531 }
BETZtechnik 8:6bb07a893f53 532 int tempToolNo = (Tens*10)+100+Ones;
BETZtechnik 8:6bb07a893f53 533 dir=1;
BETZtechnik 8:6bb07a893f53 534 display.printf("T");
BETZtechnik 8:6bb07a893f53 535 wait(0.05);
BETZtechnik 8:6bb07a893f53 536 display.printf("%d", tempToolNo);
BETZtechnik 8:6bb07a893f53 537 wait(0.001);
BETZtechnik 8:6bb07a893f53 538 dir=0;
BETZtechnik 8:6bb07a893f53 539 index = 0;
BETZtechnik 8:6bb07a893f53 540 tempToolNo=0;
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 8:6bb07a893f53 548 // *****************************************************************************
BETZtechnik 5:18c6ff1370d0 549
BETZtechnik 0:09419d572e90 550
BETZtechnik 2:79c03ac27b72 551
BETZtechnik 2:79c03ac27b72 552 }
BETZtechnik 2:79c03ac27b72 553 }