Jack Hansdampf / MotorinoLib

Dependents:   Motorino_Lib xxx_Motorino_Lib

Motorino.cpp@0:9d4d08d4e918, 2021-09-28 (annotated)

Committer:
jack1930
Date:
Tue Sep 28 15:44:00 2021 +0000
Revision:
0:9d4d08d4e918
Version 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jack1930 0:9d4d08d4e918 1 #include "Motorino.h"
jack1930 0:9d4d08d4e918 2
jack1930 0:9d4d08d4e918 3 Motorino* Motorino::meinMotorino=NULL;
jack1930 0:9d4d08d4e918 4
jack1930 0:9d4d08d4e918 5 Motorino::Motorino(PinName sda, PinName scl)
jack1930 0:9d4d08d4e918 6 {
jack1930 0:9d4d08d4e918 7 motorino=new I2C(sda,scl);
jack1930 0:9d4d08d4e918 8 init();
jack1930 0:9d4d08d4e918 9 }
jack1930 0:9d4d08d4e918 10 Motorino* Motorino::gibMotorino()
jack1930 0:9d4d08d4e918 11 {
jack1930 0:9d4d08d4e918 12 printf("gibMotorino\r\n");
jack1930 0:9d4d08d4e918 13 if (meinMotorino==NULL)
jack1930 0:9d4d08d4e918 14 {
jack1930 0:9d4d08d4e918 15 meinMotorino=new Motorino(SDA,SCL);
jack1930 0:9d4d08d4e918 16 }
jack1930 0:9d4d08d4e918 17 return meinMotorino;
jack1930 0:9d4d08d4e918 18
jack1930 0:9d4d08d4e918 19 }
jack1930 0:9d4d08d4e918 20 char Motorino::getLED_ON_L(int nr)
jack1930 0:9d4d08d4e918 21 {
jack1930 0:9d4d08d4e918 22 return LED0_ON_L+4*nr;
jack1930 0:9d4d08d4e918 23 }
jack1930 0:9d4d08d4e918 24
jack1930 0:9d4d08d4e918 25 char Motorino::getLED_ON_H(int nr)
jack1930 0:9d4d08d4e918 26 {
jack1930 0:9d4d08d4e918 27 return LED0_ON_L+4*nr+1;
jack1930 0:9d4d08d4e918 28 }
jack1930 0:9d4d08d4e918 29
jack1930 0:9d4d08d4e918 30 char Motorino::getLED_OFF_L(int nr)
jack1930 0:9d4d08d4e918 31 {
jack1930 0:9d4d08d4e918 32 return LED0_ON_L+4*nr+2;
jack1930 0:9d4d08d4e918 33 }
jack1930 0:9d4d08d4e918 34
jack1930 0:9d4d08d4e918 35 char Motorino::getLED_OFF_H(int nr)
jack1930 0:9d4d08d4e918 36 {
jack1930 0:9d4d08d4e918 37 return LED0_ON_L+4*nr+3;
jack1930 0:9d4d08d4e918 38 }
jack1930 0:9d4d08d4e918 39
jack1930 0:9d4d08d4e918 40 void Motorino::WriteToASpecificRegister(char RegAdr,char Wert)
jack1930 0:9d4d08d4e918 41 {
jack1930 0:9d4d08d4e918 42 char daten[2];
jack1930 0:9d4d08d4e918 43 daten[0]=RegAdr;
jack1930 0:9d4d08d4e918 44 daten[1]=Wert;
jack1930 0:9d4d08d4e918 45 motorino->write(adresse,daten, 2, 0);
jack1930 0:9d4d08d4e918 46 }
jack1930 0:9d4d08d4e918 47
jack1930 0:9d4d08d4e918 48 void Motorino::setDutyCycle(float dtc, int lednr)
jack1930 0:9d4d08d4e918 49 {
jack1930 0:9d4d08d4e918 50 unsigned int on,off;
jack1930 0:9d4d08d4e918 51 unsigned char onL,offL,onH,offH;
jack1930 0:9d4d08d4e918 52 on=0;
jack1930 0:9d4d08d4e918 53 off=4095*dtc/100;
jack1930 0:9d4d08d4e918 54 onL=on&0xFF;
jack1930 0:9d4d08d4e918 55 onH=on>>8&0xf;
jack1930 0:9d4d08d4e918 56 offL=off&0xFF;
jack1930 0:9d4d08d4e918 57 offH=off>>8&0xf;
jack1930 0:9d4d08d4e918 58 WriteToASpecificRegister(getLED_ON_L(lednr),onL);
jack1930 0:9d4d08d4e918 59 WriteToASpecificRegister(getLED_ON_H(lednr),onH);
jack1930 0:9d4d08d4e918 60 WriteToASpecificRegister(getLED_OFF_L(lednr),offL);
jack1930 0:9d4d08d4e918 61 WriteToASpecificRegister(getLED_OFF_H(lednr),offH);
jack1930 0:9d4d08d4e918 62 }
jack1930 0:9d4d08d4e918 63
jack1930 0:9d4d08d4e918 64 void Motorino::init()
jack1930 0:9d4d08d4e918 65 {
jack1930 0:9d4d08d4e918 66 printf("Motorino Init\r\n");
jack1930 0:9d4d08d4e918 67 WriteToASpecificRegister(MODE1,SLEEP);
jack1930 0:9d4d08d4e918 68 WriteToASpecificRegister(PRE_SCALE,121); //50Hz Wert=Runden(25MHz/4096/50)-1
jack1930 0:9d4d08d4e918 69 WriteToASpecificRegister(MODE1,EXTCLK); //Switch to external 25MHz Clock
jack1930 0:9d4d08d4e918 70
jack1930 0:9d4d08d4e918 71 setDutyCycle(25,0);
jack1930 0:9d4d08d4e918 72
jack1930 0:9d4d08d4e918 73 }
jack1930 0:9d4d08d4e918 74
jack1930 0:9d4d08d4e918 75 void Motorino::servo(float pos,int nr,float start, float end) //pos 0..180°
jack1930 0:9d4d08d4e918 76 {
jack1930 0:9d4d08d4e918 77 while(busy);
jack1930 0:9d4d08d4e918 78 busy=true;
jack1930 0:9d4d08d4e918 79 setDutyCycle(start+(end-start)*pos/180.0,nr);
jack1930 0:9d4d08d4e918 80 busy=false;
jack1930 0:9d4d08d4e918 81 }