pico i2c demo with pca9685

pico_pca9685_demo.txt

/*
 * 
 * demo of pico using pca9685 without library
 * this is for 2 rc servo
 * verified with pulseview march 4, 2024
 * 16 bit register content below

1490 us ------ center
on  0x0000
off 0x013B

1090 us ------ one way
on  0x0000
off 0x00E6

1980 us  ----- other way
on  0x0000
off 0x01A4
*/
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"

#define I2C_PORT i2c0
#define I2C_SDA 12
#define I2C_SCL 13
#define SERVO1 0x06
#define SERVO2 0x0A

void init(){
  uint8_t buf2[2];
  uint8_t buf3[3];
  uint8_t data;
  
  buf2[0]=0x00;      //mode1
  buf2[1]=0b00110001;//sleep and change pwm frequency
  i2c_write_blocking(i2c0,0x40,buf2,2,false);
  sleep_ms(500);
  
  buf2[0]=0xfe; //pre scale register
  buf2[1]=0x79; //51hz per pulseview
  i2c_write_blocking(i2c0,0x40,buf2,2,false);
  sleep_ms(500);
  
  buf2[0]=0x00; //mode1
  buf2[1]=0b10100001;
  i2c_write_blocking(i2c0,0x40,buf2,2,false);
  sleep_ms(500);}    

void i2c_write_byte(uint8_t reg, uint8_t data){
  uint8_t buf[2];
  buf[0]=reg;
  buf[1]=data;
  i2c_write_blocking(i2c0,0x40,buf,2,false);}
  
void i2c_write_2byte(uint8_t reg, uint8_t low,uint8_t high){
  uint8_t buf[5];
  buf[0]=reg;
  buf[1]=0x00;
  buf[2]=0x00;
  buf[3]=low;
  buf[4]=high;
  i2c_write_blocking(i2c0,0x40,buf,5,false);}
	
int main(){
  i2c_init(I2C_PORT, 400*1000);
  gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
  gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
  gpio_pull_up(I2C_SDA);
  gpio_pull_up(I2C_SCL);
  init();
    
for(;;){
i2c_write_2byte(SERVO1,0x3B,0x01); //center
i2c_write_2byte(SERVO2,0x3B,0x01);
sleep_ms(3000);
i2c_write_2byte(SERVO1,0xE6,0x00); //one way
i2c_write_2byte(SERVO2,0xA4,0x01);
sleep_ms(3000);
i2c_write_2byte(SERVO1,0x3B,0x01); //center
i2c_write_2byte(SERVO2,0x3B,0x01);
sleep_ms(3000);
i2c_write_2byte(SERVO1,0xA4,0x01); //other way
i2c_write_2byte(SERVO2,0xE6,0x00);
sleep_ms(3000);  
} return 0;}