Jack Hansdampf / Mbed OS Motorino

main.cpp

Committer:
jack1930
Date:
2021-09-24
Revision:
0:86d0148c14a6

File content as of revision 0:86d0148c14a6:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "platform/mbed_thread.h"

#define adresse 0x80
#define EXTCLK 1<<6
#define SLEEP 1<<4
#define AI 1<<5
#define PRE_SCALE 0xFE

#define MODE1 0x0
#define MODE2 0x1
#define LED0_ON_L 0x06

I2C motorino(D14,D15);
AnalogIn poti(PA_0);

// Blinking rate in milliseconds
#define BLINKING_RATE_MS                                                    50

char getLED_ON_L(int nr)
{
    return LED0_ON_L+4*nr;
}

char getLED_ON_H(int nr)
{
    return LED0_ON_L+4*nr+1;
}

char getLED_OFF_L(int nr)
{
    return LED0_ON_L+4*nr+2;
}

char getLED_OFF_H(int nr)
{
    return LED0_ON_L+4*nr+3;
}

void WriteToASpecificRegister(char RegAdr,char Wert)
{
    char daten[2];
    daten[0]=RegAdr;
    daten[1]=Wert;
    motorino.write(adresse,daten, 2, 0);
}

void setDutyCycle(float dtc, int lednr)
{
    unsigned int on,off;
    unsigned char onL,offL,onH,offH;
    on=0;
    off=4095*dtc/100;
    onL=on&0xFF;
    onH=on>>8&0xf;
    offL=off&0xFF;
    offH=off>>8&0xf;
    WriteToASpecificRegister(getLED_ON_L(lednr),onL);
    WriteToASpecificRegister(getLED_ON_H(lednr),onH);
    WriteToASpecificRegister(getLED_OFF_L(lednr),offL);
    WriteToASpecificRegister(getLED_OFF_H(lednr),offH);
}

void init()
{
    WriteToASpecificRegister(MODE1,SLEEP);
    WriteToASpecificRegister(PRE_SCALE,121); //50Hz Wert=Runden(25MHz/4096/50)-1
    WriteToASpecificRegister(MODE1,EXTCLK); //Switch to external 25MHz Clock

    setDutyCycle(25,0);
    
}

void servo(float pos,int nr,float start=5, float end=10)  //pos 0..180°
{ 
   setDutyCycle(start+(end-start)*pos/180.0,nr);    
}

int main()
{
    int dtc=0;
    // Initialise the digital pin LED1 as an output
    DigitalOut led(LED1);
    init();
    while (true) {
        led = !led;
        dtc+=1;
        if (dtc>180) dtc=0;
        servo(poti*180,0);
        thread_sleep_for(BLINKING_RATE_MS);
    }
}