File content as of revision 11:e6dda919438a:

#include "mbed.h"
#include "board.h"
#include "I2C.h"

//https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/blob/master/Adafruit_PWMServoDriver.cpp
//https://developer.mbed.org/handbook/I2C
//https://www.arduino.cc/en/Reference/WireWrite
//https://github.com/adafruit/Adafruit_Python_PCA9685/blob/master/Adafruit_PCA9685/PCA9685.py

#define SERVOMIN        150
#define SERVOMAX        600

#define ADRESS          0x40
#define MODE1           0x00
#define MODE2           0x01
#define SUBADR1         0x02
#define SUBADR2         0x03
#define SUBADR3         0x04
#define PRESCALE        0xFE
#define LED0_ON_L       0x06
#define LED0_ON_H       0x07
#define LED0_OFF_L      0x08
#define LED0_OFF_H      0x09
#define ALL_LED_ON_L    0xFA
#define ALL_LED_ON_H    0xFB
#define ALL_LED_OFF_L   0xFC
#define ALL_LED_OFF_H   0xFD

#define RESTART         0x80
#define SLEEP           0x10
#define ALLCALL         0x01
#define INVRT           0x10
#define OUTDRV          0x04

I2C i2c(PB_9, PB_8);



void init_servo(int freq)
{
    char data[2];

    //Reset
    data[0] = (char) MODE1;
    data[1] = (char) SLEEP;  
    i2c.write((ADRESS<<1), data,2);
    
    wait_ms(1);
    
    float prescaleval = 25000000.0;     //25MHz
    prescaleval /= 4096.0f;             //12-Bit
    prescaleval /= (float)freq;
    prescaleval -= 1.0f;
    char prescale = (char)(prescaleval);    //0x64 bei 50Hz
    
    data[0] = (char) PRESCALE;
    data[1] = prescale;
    i2c.write((ADRESS<<1), data,2);
    
    wait_ms(1);
    
    data[0] = (char) MODE1;
    data[1] = 0x80;             //do restart 
    i2c.write((ADRESS<<1), data,2);
    
    wait_ms(1);
    
    data[0] = (char) MODE2;
    data[1] = (char) 0x04;
    i2c.write((ADRESS<<1), data,2);
    
    wait_ms(1);

    //Servo_enable = 1;
    printf("init done...\r\n");
}

//servo => channel No. deg => angle 0°...150°
void set_servo_position(int servo, int deg)
{
    deg = SERVOMIN + (3 * deg);
    if (deg > SERVOMAX) {
        deg = SERVOMAX;
    }
    char data[8];
    int16_t wert1 = (deg>>8);
    int16_t wert2 = (deg & 0xFF);
    printf("%x %x servo deg\r\n",wert1,wert2);
    data[0] = (char)LED0_ON_L+(4*servo);
    data[1] = 1;
    data[2] = (char)LED0_ON_H+(4*servo); 
    data[3] = 0x00;
    data[4] = (char)LED0_OFF_L+(4*servo);
    data[5] = (char)wert2;
    data[6] = (char)LED0_OFF_H+(4*servo); 
    data[7] = (char)wert1;
    i2c.write((ADRESS<<1), data,8);
}

int getPWM(uint8_t servo) 
{
    char read = 255;
    char data;
    data = (char)(LED0_OFF_L+(4*servo));
    i2c.write((ADRESS<<1), &data, 1, 1);
    i2c.read((ADRESS<<1), &read, 1, 0);
    printf(">>%x<< read from servo %d\r\n",read,servo);
    return (int)read;
}

int getPRESCALE(void) 
{
    char read = 255;
    char data;
    data = (char)(PRESCALE);
    i2c.write((ADRESS<<1), &data, 1, 1);
    i2c.read((ADRESS<<1), &read, 1, 0);
    printf("read Prescale value: >>%x<<\r\n",read);
    return (int)read;
}