Well the speed control didnt take very long at all to implement so I have attached a fully working code below.
Now i have to wait for the servo motors i have ordered to arrive from HK before i move on to having 2 servos in control at once!
any advice on different ways of doing things are welcome.
also sorry for the untidyness of the code but i shall neatn it up in the near future with comments on code
Code:'******************************************************************************** '* college project to control 2 servo motors from a 3 axis accelerometer * '******************************************************************************** '* 04/03/09 CODE WRITTEN BY CHRIS HAMMERSLEY * ' FEEL FREE TO MAKE USE OF THIS CODE BUT PLEASE GIVE CREDIT TO WHERE IT IS DUE * ' V 0.4 ' For loop responding to input of ADC ' active ADC inputs displayed on line 2 of lcd ' servo position shown on line 1 of LCD ' removed twitch by adding a minimum movement to adjust servo ' more reliable tracking ' QUICKER RESPONCE TO CHANGES IN AXIS BY REDUCTION OF FOR LOOPS ' SERVO MOVEMENT NOW REPRESENTS DIFFERENT SPEEDS IN MOVEMENT '*********************************************************************** '** Shows the Connections on the ports of the PIC ********************** '*********************************************************************** ' 'A0 = X-AXIS 'A1 = Y-AXIS 'A2 = Z-AXIS 'A3 = 'A4 = 'A5 = ' 'BO = LCD INTERFACE 'B1 = LCD INTERFACE 'B2 = LCD INTERFACE 'B3 = LCD INTERFACE 'B4 = LCD INTERFACE 'B5 = LCD INTERFACE 'B6 = LCD INTERFACE 'B7 = LCD INTERFACE ' 'C0 = SERVO 1 'C1 = SERVO 2 'C2 = 'C3 = 'C4 = 'C5 = 'C6 = 'C7 = '*********************************************************************** '****************** DEFINE ADCIN PARAMETERS***************************** '*********************************************************************** DEFINE ADC_BITS 8 'SETS NUMBER OF BITS IN RESULTS DEFINE ADC_CLOCK 3 'SET CLOCK SOURCE DEFINE ADC_SAMPLUS 50 'SET SAMPLING TIME IN US DEFINE LCD_DREG PORTB 'Define PIC port used for LCD Data lines DEFINE LCD_DBIT 4 'Define first pin of portb connected to LCD DB4 DEFINE LCD_RSREG PORTB 'Define PIC port used for RS line of LCD DEFINE LCD_RSBIT 3 'Define Portb pin used for RS connection DEFINE LCD_EREG PORTB 'Define PIC prot used for E line of LCD DEFINE LCD_EBIT 0 'Define PortB pin used for E connection DEFINE LCD_BITS 4 'Define the 4 bit communication mode to LCD DEFINE LCD_LINES 2 'Define using a 2 line LCD DEFINE LCD_COMMANDUS 2000 'Define delay between sending LCD commands DEFINE LCD_DATAUS 50 'Define delay time between data sent. SERVOY VAR PORTC.0 SERVOX VAR PORTC.1 YAXIS VAR WORD XAXIS VAR WORD ZAXIS VAR WORD XPRESET VAR WORD YPRESET VAR WORD ZPRESET VAR WORD B0 VAR WORD B1 VAR WORD B2 VAR WORD B3 VAR WORD STEPPING VAR WORD TRISA = %00000011 'SETS INPUTS/OUTPUTS FOR BANK A TRISB = %00000000 'SETS INPUTS/OUTPUTS FOR BANK B TRISC = %00000000 'SETS INPUTS/OUTPUTS FOR BANK C ADCON1 = %00000110 'SETS THE ANALOGUE/DIGITAL INPUTS TO BANK A PORTA = %00000000 'SETS THE INITIAL VALUE OF BANK A PORTB = %00000000 'SETS THE INITIAL VALUE OF BANK B PORTC = %00000000 'SETS THE INITIAL VALUE OF BANK C '************************************************************************** ' CENTER THE SERVO ' **************** ' Pause for one second to allow initalisations ' Clear the LCD display ' Center Servo 1 and then servo 2 ' '************************************************************************** PAUSE 1000 LCDOUT "STARTING" pause 1000 LCDOUT $FE, 1 CENTER: 'NEW SUBROUTINE FOR SETTING THE SERVO FOR B0 = 1 TO 100 'MOVES THE SERVO FROM ITS 0 TO ITS 100 MARK PULSOUT SERVOY, 150 ' PAUSE 20 ' NEXT FOR B1 = 1 TO 100 'Pans the servo from 0 to a halfway point of 100 PULSOUT SERVOX, 150 ' PAUSE 20 ' NEXT '************************************************************************** ' setup ADC ' ********* ' reads the ADC ports and sets the initial values for calculations '************************************************************************** STARTUP: ADCIN 0, B2 ' reads in the ADC on PORT A.0 and sends it to memory location B2 ADCIN 1, B3 ' reads in the ADC on PORT A.1 and sends it to memory location B3 STEPPING = 1 gosub LCDUPDATE '************************************************************************** ' Servo calculations ' ****************** ' ' '************************************************************************** MAIN: gosub LCDUPDATE ADCIN 0, B2 ' Reads in the PORT A.0 ADC AND STORES THE VALUE IN B2 ADCIN 1, B3 ' READS IN THE PORT a.1 ADC AND STORES THE VALUE IN B3 XPRESET = B2 + 50 If Xpreset > B0 then goto positiveone endif if xpreset < B0 then goto negativeone endif '******************************************************************* ' SERVO MOVEMENT ' ************** ' ' CONTROLS THE OUTPUTS TO THE SERVO ' '******************************************************************** positive: FOR B0 = B0 TO XPRESET STEP STEPPING PULSOUT SERVOY, B0 PAUSE 20 gosub LCDUPDATE NEXT goto main negative: FOR B0 = B0 TO xpreset STEP - STEPPING PULSOUT SERVOY, B0 PAUSE 20 gosub LCDUPDATE NEXT goto main '****************************************************************************** ' MOVEMENT CALCULATIONS ' ********************* ' ' THIS SECTION COMPARES THE INPUT TO THE CURRENT LOCATION AND DECIDES WHETHER A ' POSITIVE OR NEGATIVE MOVEMENT IS REQUIRED ' '******************************************************************************* positiveone: xaxis= xpreset - B0 if xaxis > 3 then goto positivetwo endif STEPPING = 1 goto main negativeone: xaxis= B0 - xpreset if xaxis > 3 then goto negativetwo endif STEPPING = 1 goto main '************************************************************************************** ' SERVO SPEED CONTROL POSITIVE ' **************************** ' ' THIS SECTION LOOKS AT THE INPUT DATA AND DECIDES HOW FAST TO MOVE THE SERVO DEPENDING ' ON THE VALUE OF THE CHANGE BETWEEN THE INPUT AND THE CURRENT POSITION ' '*************************************************************************************** positivetwo: IF XAXIS > 18 THEN STEPPING = 6 ENDIF IF XAXIS > 15 THEN STEPPING = 5 ENDIF IF XAXIS > 12 THEN STEPPING = 4 ENDIF IF XAXIS > 9 THEN STEPPING = 3 ENDIF IF XAXIS > 6 THEN STEPPING = 2 ELSE STEPPING = 1 ENDIF '************************************************************************************************************** ' THE FOLLOWING CODE DIVIDES THE CHANGE VALUE BY 4 SO THAT THE FOR LOOP ONLY CHANGES A 4TH OF THE VALUE BEFORE ' RECHECKING ITS INPUTS '************************************************************************************************************** if xaxis > 20 then xaxis = xaxis / 4 XPRESET = B0 + XAXIS ENDIF goto positive '************************************************************************************** ' SERVO SPEED CONTROL NEGATIVE ' **************************** ' ' THIS SECTION LOOKS AT THE INPUT DATA AND DECIDES HOW FAST TO MOVE THE SERVO DEPENDING ' ON THE VALUE OF THE CHANGE BETWEEN THE INPUT AND THE CURRENT POSITION ' '*************************************************************************************** negativetwo: IF XAXIS > 18 THEN STEPPING = 6 ENDIF IF XAXIS > 15 THEN STEPPING = 5 ENDIF IF XAXIS > 12 THEN STEPPING = 4 ENDIF IF XAXIS > 9 THEN STEPPING = 3 ENDIF IF XAXIS > 6 THEN STEPPING = 2 ELSE STEPPING = 1 ENDIF '************************************************************************************************************** ' THE FOLLOWING CODE DIVIDES THE CHANGE VALUE BY 4 SO THAT THE FOR LOOP ONLY CHANGES A 4TH OF THE VALUE BEFORE ' RECHECKING ITS INPUTS '************************************************************************************************************** IF XAXIS > 20 THEN XAXIS = XAXIS / 4 XPRESET = B0 - XAXIS ENDIF GOTO NEGATIVE ' ' ''******************************************************************************************************************* ' LCDUPDATE CONTROL ' ********************* ' MOVES BOTH SERVOS FOR BOTH X AND Y MOVEMENT ' CREATES THE NEW X AND Y PRESETS FOR THE CALCULATIONS ' UPDATES THE LCD DISPLAY '******************************************************************************************************************* LCDUPDATE: ADCIN 0, B2 ' Reads in the PORT A.0 ADC AND STORES THE VALUE IN B2 ADCIN 1, B3 ' READS IN THE PORT a.1 ADC AND STORES THE VALUE IN B3 LCDOUT $FE, 2 ' SENDS THE COMMAND TO THE LCD TO MOVE TO THE START OF THE FIRST LINE LCDOUT "Servo Pos= ",DEC3 B0 ' SENDS THE DATA FOR THE X AXIS TO THE LCD ON THE FIRST LINE LCDOUT $FE, $C0 ' MOVES THE CURSUR OF THE LCD TO THE START OF THE SECOND LINE LCDOUT "Y = ",DEC3 B2, " X = ",DEC3 B3 ' SENDS THE DATA FOR THE Y AXIS TO THE LCD ON THE SECOND LINE RETURN ' RETURNS TO THE PREVIOUS BLAAAAAAAAAAAAAAAAAAAAAAH 'GOTO MAIN END*
Bookmarks