Diff: Motorino.cpp

Revision:

0:9d4d08d4e918

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motorino.cpp	Tue Sep 28 15:44:00 2021 +0000
@@ -0,0 +1,81 @@
+#include "Motorino.h"
+
+Motorino* Motorino::meinMotorino=NULL;
+
+Motorino::Motorino(PinName sda, PinName scl)
+{
+    motorino=new I2C(sda,scl);
+    init();
+}
+Motorino* Motorino::gibMotorino()
+    {
+        printf("gibMotorino\r\n");
+        if (meinMotorino==NULL)
+            {
+             meinMotorino=new Motorino(SDA,SCL);
+            }
+        return meinMotorino;   
+        
+    }
+char Motorino::getLED_ON_L(int nr)
+{
+    return LED0_ON_L+4*nr;
+}
+
+char Motorino::getLED_ON_H(int nr)
+{
+    return LED0_ON_L+4*nr+1;
+}
+
+char Motorino::getLED_OFF_L(int nr)
+{
+    return LED0_ON_L+4*nr+2;
+}
+
+char Motorino::getLED_OFF_H(int nr)
+{
+    return LED0_ON_L+4*nr+3;
+}
+
+void Motorino::WriteToASpecificRegister(char RegAdr,char Wert)
+{
+    char daten[2];
+    daten[0]=RegAdr;
+    daten[1]=Wert;
+    motorino->write(adresse,daten, 2, 0);
+}
+
+void Motorino::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 Motorino::init()
+{
+    printf("Motorino Init\r\n");
+    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 Motorino::servo(float pos,int nr,float start, float end)  //pos 0..180°
+{ 
+    while(busy);
+    busy=true;
+    setDutyCycle(start+(end-start)*pos/180.0,nr);  
+    busy=false;
+}
\ No newline at end of file