disco4now Guru
Posted: 07:37am 03 Apr 2018
Copy link to clipboard Print this post
DIYODE Robot thingy Quote '------------------------------------------------------------------------------' ' Micromite library for PCA9685 ' ' Uses PCA9685 16-channel PWM over I2C to support 16 Servos or PWM pins ' ' ' ' ' ' Author: Disco4now TBS Forum ' ' ' dim ProgTitle$ = "PCA9685 Test Suite" ' ' ' dim ProgVer$ = "v1.0.0" ' ' ' dim ProgDate$ = "03-Apr-2018" ' '------------------------------------------------------------------------------' 'Code for a Micromite to communicate with a PCA9685 16-channel PWM over I2C. 'Also contains routine to allow calibration of servos. 'The console input is scanned and characters read as they are typed ' ServoMIN(16),ServoMAX(16) hold min and max pulse widths for 0 and 180 deg respectively. ' nominal values 800 and 2200 but adjust to suit your particular servos ' calibration values for each servo ' 0-9,a-f used to select the servo to calibrate. ' SPACEBAR sets servo near the centre i.e 90 deg for nominal range of 0-180 ' right arrow sets servo to 180 deg.SPACEBAR to reurn to centre. ' PgUp/PgDn increase/decrease pulse width by 10ms. Up/Down Arrows increase/decrease by 1 ms ' Press SPACEBAR and current value will print to console. ' When the servo is operating correctly then record the max/min values and used them ' to set ServoMIN(x) and ServoMAX(x) values in the PWMInit() routine. ' nominal values 800 and 2200 but adjust to suit your servos ' e.g. ' ServoMIN(0)=480:ServoMAX(0)=2140 ' ServoMIN(1)=480:ServoMAX(1)=2085 ' ServoMIN(2)=460:ServoMAX(2)=2125 ' Once these are set you just use ' SERVOSet(id,angle) ' to set individual servo to desired angle Option Explicit Option Default NONE '================================================================================================ ' Data Variables '================================================================================================ dim integer loc0last,loc0now,loc1last,loc1now,loc2last,loc2now,GetCon= 0 ,GetCom1= 0 ,GetCom2= 0 dim integer i,id= 0 ,highlow= 0 '================================================================================================ ' Program Initialization '================================================================================================ Print "Starting " + ProgTitle$ + ": " + ProgVer$ + " - " + ProgDate$ settick 100 ,IsSerialOrConsoleComplete, 1 'Initialise the PCA9685 16-channel PWM over I2C at frequency of 1000 PWMInit 50 '50Hz for 20ms pulse period represented by 4096 ticks 'Uncomment to sweep servos at start up. '================================================================================================ ' Main program loop '================================================================================================ do 'Watchdog 40000 'reset after 40 secs on hang '==============================Check Console and Serial Ports==================================== IF GetCon= 1 then 'If data in console1 then GOSUB GetConsole END IF IF GetCom1= 1 then 'If data in COM1 then GOSUB GetSerial1 END IF IF GetCom2= 1 then 'If data in COM2 then GOSUB GetSerial2 END IF Loop '================================================================================================ ' Procedures providing program functionality components '================================================================================================ 'Checks console and serial ports for data and sets flag if data has stopped coming in. 'flag triggers read of the data in the main program loop sub IsSerialOrConsoleComplete 'Check console and set flag if data exists and has stopped coming in loc0now= LOC (#0) IF Loc0now> 0 then IF loc0now=loc0last then 'data finished GetCon= 1 'If data in CON then set GetConsole flag else ENDIF END IF 'Check COM1 and set flag if data exists and has stopped coming in 'Check COM2 and set flag if data exists and has stopped coming in END sub 'Reads data from the console sub ReadConsole local char$ local integer i,j,k LOC (#0) INPUT$ ( 100 ,#0) IF char$<> "" then IF len (char$)= 1 then ' ? char$; ' ? ASC(left$(char$,1)); IF Char$= "0" then id= 0 IF Char$= "1" then id= 1 IF Char$= "2" then id= 2 IF Char$= "3" then id= 3 IF Char$= "4" then id= 4 IF Char$= "5" then id= 5 IF Char$= "6" then id= 6 IF Char$= "7" then id= 7 IF Char$= "8" then id= 8 IF Char$= "9" then id= 9 IF Char$= "a" then id= 10 IF Char$= "b" then id= 11 IF Char$= "c" then id= 12 IF Char$= "d" then id= 13 IF Char$= "e" then id= 14 IF Char$= "f" then id= 15 IF Char$= chr$ ( 145 ) then 'F1 VT100 ? "F1" END IF IF Char$= chr$ ( 146 ) then 'Home VT100 ? "F2" END IF IF Char$= chr$ ( 147 ) then 'Home VT100 ? "F3" END IF IF Char$= chr$ ( 148 ) then 'Home VT100 ? "F4" END IF IF Char$= chr$ ( 149 ) then 'F1 VT100 ? "F5" END IF IF Char$= chr$ ( 150 ) then 'Home VT100 ? "F6" END IF IF Char$= chr$ ( 151 ) then 'Home VT100 ? "F7" END IF IF Char$= chr$ ( 152 ) then 'Home VT100 ? "F8" END IF IF Char$= chr$ ( 153 ) then 'Home VT100 ? "F9" END IF IF Char$= chr$ ( 155 ) then 'Home VT100 ? "F11" END IF IF Char$= chr$ ( 156 ) then 'Home VT100 ? "F12" END IF IF Char$= chr$ ( 127 ) then 'up arrow VT100 ? "Del" END IF IF Char$= chr$ ( 128 ) then 'up arrow VT100 '? "Up Arrow" IF highlow= 0 then 1 else 1 END IF END IF IF Char$= chr$ ( 129 ) then 'down arrow '? "Down Arrow" IF highlow= 0 then 1 else 1 END IF END IF IF Char$= chr$ ( 131 ) or Char$= chr$ ( 4 ) then 'right arrow VT100 MMEdit '?"Right Arrow" SERVOSet(id , 180 ) 1 END IF IF Char$= chr$ ( 130 ) or Char$= chr$ ( 19 ) then 'left arrow VT100 VT100 MMEdit '? "Left Arrow" SERVOSet(id , 0 ) 0 END IF IF Char$= chr$ ( 132 ) then "INS" END IF IF Char$= chr$ ( 133 ) then "133" END IF IF Char$= chr$ ( 134 ) then "Home" END IF IF Char$= chr$ ( 135 ) then "End" END IF IF Char$= chr$ ( 136 ) then 'PgUp VT100 IF highlow= 0 then 10 else 10 END IF END IF IF Char$= chr$ ( 137 ) then 'PgUp VT100 IF highlow= 0 then 10 else 10 END IF END IF IF Char$= chr$ ( 138 ) then "138" END IF IF Char$= chr$ ( 139 ) then "ALT" END IF IF Char$= chr$ ( 140 ) then "140" END IF IF Char$= " " then ' ? "SPACE BAR" SERVOSet(id , 90 ) IF highlow= 1 then " High(180)" else " Low(0)" ENDIF ENDIF else ASC ( left$ (char$, 1 )); ENDIF IF ASC ( left$ (char$, 1 ))= 27 then "ESC" END IF END IF END IF END sub '================================================================================================ ' PCA9685 I2C PWM Routines '================================================================================================ 'Code for a Micromite to communicate with a PCA9685 16-channel PWM over I2C. 'Original Author:- Limor Fried/Ladyada for Adafruit Industries 'Ported to MMBasic by Disco4now!, TBS forums '********************************************************************************** ' This is a library for our Adafruit 16-channel PWM & Servo driver ' Pick one up today in the adafruit shop! ' ------> http://www.adafruit.com/products/815 ' These displays use I2C to communicate, 2 pins are required to ' interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4 ' Adafruit invests time and resources providing this open source code, ' please support Adafruit and open-source hardware by purchasing ' products from Adafruit! ' Written by Limor Fried/Ladyada for Adafruit Industries. ' BSD license, all text above must be included in any redistribution '********************************************************************************** sub SERVOSet(id as integer ,angle as integer ) 'set individual servo to desired angle local integer tickoff 4096 *(ServoMin(id)+(ServoMAX(id)-ServoMIN(id))*angle/ 180 )/ 20000 '? tickoff PWMSet(id , 1 ,tickoff) END sub sub PWMSetPin(id as integer ,value as integer ,inv as integer ) 'Helper to set pin PWM output. Sets pin without having to deal with on/off tick placement 'and properly handles a zero value as completely off and 4095 as completely on. 'Optional invert parameter supports inverting the pulse for sinking to ground. 'id is PWM output pin, from 0 to 15 'value is the number of ticks out of 4096 to be active, should be a value from 0 to 4095 inclusive. 'inv if true, inverts the output, defaults to 'false' 'clamp value between 0 and 4095 inclusive. IF value > 4095 then value= 4095 IF value < 0 then value= 0 IF inv= 0 then IF value= 0 then 'special value for fully off PWMSet(id , 4096 , 0 ) else IF value= 4095 then 'special value for fully on PWMSet(id , 0 , 4096 ) else 0 , 4095 -value); ENDIF else IF value= 4095 then 4096 , 0 ) else IF value= 0 then 0 , 4096 ) else 0 , value); ENDIF ENDIF END sub sub PWMSet(id as integer ,ontick as integer ,offtick as integer ) 'main routine -sets the PWM output of one of the PCA9685 pins ' id - One of the PWM output pins, from 0 to 15 ' ontick - At what point in the 4096-part cycle to turn the PWM output ON ' offtick - At what point in the 4096-part cycle to turn the PWM output OFF i2c write PCA9685_ADDRESS, 0 , 5 ,LED0_ON_L+ 4 *id,ontick and &HFF ,ontick >> 8 ,offtick and &HFF ,offtick >> 8 IF MM.I2C then ERROR "slave did not respond in PWMset" END sub sub PWMInit(freq as FLOAT ) 'Select one of the following possible PCA9685 I2C Addresses in range 40-7F '40 is default for no links on A5-A0 address pins. 'Call once at the start of the program with desired frequency to be used on all pins dim integer PCA9685_ADDRESS = &H40 'default &H40 (&H40-&H7F) dim integer PCA9685_MODE1= &H0 dim integer PCA9685_PRESCALE= &HFE dim integer LED0_ON_L= &H6 dim integer LED0_ON_H= &H7 dim integer LED0_OFF_L= &H8 dim integer LED0_OFF_H= &H9 dim integer ServoMIN( 16 ),ServoMAX( 16 ) 'calibration values for each servo ' nominal values 800 and 2200 but adjust to suit your servos ServoMIN( 0 )= 480 :ServoMAX( 0 )= 2140 1 )= 480 :ServoMAX( 1 )= 2085 2 )= 460 :ServoMAX( 2 )= 2125 3 )= 475 :ServoMAX( 3 )= 2050 4 )= 480 :ServoMAX( 4 )= 2160 5 )= 480 :ServoMAX( 5 )= 2160 6 )= 480 :ServoMAX( 6 )= 2160 7 )= 480 :ServoMAX( 7 )= 2160 8 )= 480 :ServoMAX( 8 )= 2160 9 )= 480 :ServoMAX( 9 )= 2160 10 )= 480 :ServoMAX( 10 )= 2160 11 )= 480 :ServoMAX( 11 )= 2160 12 )= 480 :ServoMAX( 12 )= 2160 13 )= 480 :ServoMAX( 13 )= 2160 14 )= 480 :ServoMAX( 14 )= 2160 15 )= 480 :ServoMAX( 15 )= 2160 i2c open 10 , 100 'reset the PCA9685 to power on state i2c write 0 , 0 , 1 , &H06 IF MM.I2C then ERROR "slave did not respond at power eset" PAUSE 1 'reset the PCA9685 i2c write PCA9685_ADDRESS, 0 , 2 ,PCA9685_MODE1, &H80 IF MM.I2C then ERROR "slave did not respond at reset" PAUSE 1 'set pwm frequency IF freq= 0 then freq= 1000 ' set a default if not set. PWMSetFreq(freq) END sub sub PWMSetFreq(freq as FLOAT ) 'sets or changes PWM frequency. 'freq range 40-1000 hz local integer mode1,GetData( 1 ) dim FLOAT prescaleval = 25000000 / 4096 dim integer prescale,currentmode 0.9 ' Correct for overshoot in the frequency setting prescale= INT ((prescaleval/freq)- 0.5 ) i2c write PCA9685_ADDRESS, 0 , 1 , PCA9685_MODE1 i2c read PCA9685_ADDRESS, 0 , 1 , mode1 ' read current mode IF MM.I2C then ERROR "slave did not respond 2" i2c write PCA9685_ADDRESS, 0 , 2 ,PCA9685_MODE1, mode1 or &H10 ' set sleep bit so change of scaler allowed i2c write PCA9685_ADDRESS, 0 , 2 ,PCA9685_PRESCALE,prescale 'set the prescaler i2c read PCA9685_ADDRESS, 0 , 1 , GetData() ' read prescaler IF MM.I2C then ERROR "slave did not respond 3" i2c write PCA9685_ADDRESS, 0 , 2 , PCA9685_MODE1,mode1 PAUSE 1 i2c write PCA9685_ADDRESS, 0 , 2 , PCA9685_MODE1,mode1 or &HA0 'This sets the MODE1 register to turn on auto increment. END sub F4 H7 FotS F4xGT
Notice. New forum software under development. It's going to miss a few functions and look a bit ugly for a while, but I'm working on it full time now as the old forum was too unstable. Couple days, all good. If you notice any issues, please contact me.
Print this page
