' {$STAMP BS2} ' {$PBASIC 2.5} ' -----[ Title ]----------------------------------------------------------- ' ARM 5 AXIS v.1 ' Last Edited by FAROS 10-AUG-2005 για την www.GREEKBOTICS.com '**************************************************************************** ' ' Πρόγραμμα ψηφιακής καθοδήγησης ρομποτικού βραχίονα με 5 κλειδώσεις. ' Με αυτό το πρόγραμμα μπορούμε να ορίσουμε την κίνηση ενός βραχίονα ' σε μονάδες (μοίρες). Έχουμε την δυνατότητα να γράψουμε στην ΕΕPROM ' μέχρι 30 κινήσεις . ' Το πρόγραμμα ξεκινά με το κυρίως μενού στο οποίο μπορείτε να επιλέξετε ' μέρος του βραχίονα και να πειραματιστείτε ρυθμίζοντας ανάλογη γωνία ' περιστροφής του βραχίονα. Έπειτα να το σώσετε στην μνήμη. ' Μπορείτε επίσης με την επιλογή EDITOR να διορθώσετε τα δεδομένα ' από κάθε κίνηση με μεγαλύτερη ακρίβεια και ευκολία για την τοποθέτηση του βραχίονα. ' Με την επιλογή RUN μπορείτε να τρέξετε το πρόγραμμα που αποθηκεύσατε ' στην EEPROM με όλες τις κινήσεις σε Αυτόματη λειτουργία. ' ENCODER είναι το ποντεσιόμετρο τον 10ΚΩ. Η κίνηση γίνετε με SERVO τις DEA. ' '****** pontesiometrο 10kΩ ************************** SERVO DEA ************* ' ' ' _ +5 ' | ' ___ _+5 ' ___ 0,1 μF | ' | ______ | __________________ ' | | | Pin <>------|----| | ' | o---/ | (red) |____| SERVO | ' 220 | |--o-->\ | | | ' Pin <>----/\/\/-----o-----o---/ | ___|__________________| ' | |______| (black) | ' | | ' Vss ___ ___ Vss ' _ _ ' . . '***************************************************************************** ' Σημείωση:Δεν περιλαμβάνει μουστάκια + τηλεχειριστήριο συμβατό με SONY ' οι αισθητήρες IR δεν μπορούν να εντοπίσουν αντικείμενα ' Με τροποποίηση του προγράμματος δέχεται περισσότερες κινήσεις στη μνήμη. ' Το πρόγραμμα έχει δοκιμαστή με ένα ποντεσιομετρο και ένα SERVO τις DEA ' με ικανοποιητικά αποτελέσματα από την παραπάνω συνδεσμολογία. ' Σύντομα κατασκευάζετε ο βραχίονας και θα συνδεθεί με το πρόγραμμα. ' Ίσος να υπάρχουν (πρακτικά) κάποιες ατέλειες στην πρώτη έκδοση . '--------------------------variable------------------------------------ col VAR Nib row VAR Nib char VAR Byte char1 VAR Byte idx VAR Nib rcRt VAR Word rcLf VAR Word servo VAR Nib encoder VAR Byte speed VAR Word sPos VAR Word sPosold VAR Word add VAR Word strAddr VAR Byte '---------------------------------CONSTAND---------------------------- 'PotCCW CON 1 ' counter-clockwise pot input PotCW CON 0 ' clockwise pot input Scale CON $0068 servo1 CON 12 ' 1 σερβοκινητήρας βαση servo2 CON 12 ' 2 σερβοκινητήρας ώμου servo3 CON 12 ' 3 σερβοκινητήρας αγκώνα servo4 CON 12 ' 4 σερβοκινητήρας επιτελούς (καρπός) servo5 CON 12 ' 5 σερβοκινητήρας λαβίδα enc1 CON 1 ' 6 ENCODER βαση enc2 CON 1 ' 7 ENCODER ώμου enc3 CON 1 ' 8 ENCODER αγκώνα enc4 CON 1 ' 9 ENCODER επιτελούς (καρπός) enc5 CON 1 ' 10 ENCODER λαβίδα Message DATA 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 DATA 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 DATA 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 Main: strAddr = Message main2: idx = 0 PAUSE 1000 DEBUG CLS DEBUG CLS," ** ------ MENΟΥ ------ **" DEBUG CR, " ** 1. ΒΑΣΗ **" DEBUG CR, " ** 2. ΒΡΑΧΙΟΝΑΣ ΩΜΟΥ **" DEBUG CR, " ** 3. ΣΥΝΔΕΣΗ ΑΓΚΩΝΑ **" DEBUG CR, " ** 4. ΒΡΑΧΙΟΝΑΣ ΕΠΙΤΕΛΟΥΣ **" DEBUG CR, " ** 5. ΛΑΒΙΔΑ **" DEBUG CR, " ** 6. RUN **" DEBUG CR, " ** 7. EDITOR **" DEBUGIN DEC1 idx 'DEBUG CR,"idx: ", DEC idx, " " ON idx GOTO main2, ser1, ser2, ser3,ser4,hand,rcRead_servo,editor ' if idx = 0..6 goto label DEBUG CR,"ON..GOTO target error.", CR ' message if idx is out of range mode: editor: DEBUG CR, "** ΠΡΟΒΟΛΗ ΜΝΗΜΗΣ **" PAUSE 500 Setup: DEBUG CLS col = 0 strAddr = Message DO WHILE (col < 10) ' attempt 10 iterations FOR row = 0 TO 15 ' attempt 16 iterations IF (row > 9) THEN EXIT ' terminate when row > 9 READ STrAddr, char ' read byte from EEPROM strAddr = strAddr + 1 ' point to next character READ STrAddr, char1 ' read byte from EEPROM strAddr = strAddr + 1 IF (char < 0) THEN EXIT DEBUG 2, (col * 12), row, ' print col/row at location DEC strAddr-2,"M " ,DEC char , ":", DEC char1, CR NEXT col = col + 1 ' update column IF (col = 3) THEN EXIT ' terminate when col = 3 LOOP edit_a: PAUSE 1000 DEBUG CLRDN ,CRSRXY,0,15," 1. ΑΛΛΑΓΗ" DEBUG CR, " 2. ΑΝΑΝΕΩΣΗ ΜΝΗΜΗΣ " DEBUG CR, " 3. ΕΞΟΔΟΣ" DEBUGIN DEC1 idx ON idx GOTO main2,edit1,Setup,main2 DEBUG "Λάθος Επιλογή" edit1: idx=1 DEBUG CLRDN ,CR," SERVO? 1-5 " ' επέλεξε αριθμό σέρβο DEBUGIN DEC1 servo DEBUG DEC servo DEBUG CR, " Pos? 1-250 " ' επέλεξε αριθμό περιστροφής DEBUGIN DEC3 sPosold DEBUG DEC sPosold GOTO write_sPos1 ser1: servo=servo1 encoder=enc1 DEBUG CLS,"1 ΒΑΣΗ" PAUSE 200 GOTO RCservo ser2: servo=servo2 encoder=enc2 DEBUG CLS, "2 ΒΡΑΧΙΟΝΑΣ ΩΜΟΥ" PAUSE 200 GOTO RCservo ser3: servo=servo3 encoder=enc3 DEBUG CLS,"3 ΣΥΝΔΕΣΗ ΑΓΚΩΝΑ" PAUSE 500 GOTO RCservo ser4: servo=servo4 encoder=enc4 DEBUG CLS, "4 ΒΡΑΧΙΟΝΑΣ ΕΠΙΤΕΛΟΥΣ" PAUSE 500 GOTO RCservo hand: servo=servo5 encoder=enc5 DEBUG CLS," ΛΑΒΙΔΑ " PAUSE 500 GOTO RCservo play: 'PAUSE 200 'DEBUG CLS," Play DATA !!!" 'PAUSE 500 GOTO rcRead_servo RCservo: DEBUG CLS, "Δώσε γωνία περιστροφής" DEBUGIN SNUM sPosold IF sPosold>250 THEN error idx=0 RCservo_1: HIGH PotCW ' discharge caps 'HIGH PotCCW PAUSE 1 RCTIME PotCW, 1, rcRt ' read clockwise 'RCTIME PotCCW, 1, rcLf ' read counter-clockwise rcRt = (rcRt */ Scale) MAX 250 ' scale RCTIME to 0-250 'rcLf = (rcLf */ Scale) MAX 250 sPos = rcRt - rcLf ' calculate position (-250 to 250) PULSOUT Servo, speed PAUSE 20 'DEBUG CLS DEBUG HOME, "Position: ", SDEC sPos, " " DEBUG CR,SDEC? sPosold, " " IF sPos>sPosold THEN speed_1 IF sPos