Peter Betz / Mbed 2 deprecated accuBlast_inputs

SINGLE Inputs code

Dependencies:   mbed reScale USBDevice

main.cpp@6:40499bfc9ab2, 2019-03-29 (annotated)

Committer:
BETZtechnik
Date:
Fri Mar 29 06:16:09 2019 +0000
Revision:
6:40499bfc9ab2
Parent:
5:6bb07a893f53
Child:
7:0c2e51da8789
READY FOR RELEASE

Who changed what in which revision?

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