Mark and Peter / Mbed 2 deprecated 11u68Displaytest

Remote inc coolant nozzle control program

Dependencies:   mbed reScale USBDevice

Dependents:   Nozzle_inputs Nozzle_display

main.cpp@19:0c2e51da8789, 2019-04-07 (annotated)

Committer:
BETZtechnik
Date:
Sun Apr 07 15:16:14 2019 +0000
Revision:
19:0c2e51da8789
Parent:
18:40499bfc9ab2 
Freezing after 31 min;

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