Motorino.cpp

00001 #include "Motorino.h"
00002 
00003 Motorino* Motorino::meinMotorino=NULL;
00004 
00005 Motorino::Motorino(PinName sda, PinName scl)
00006 {
00007     motorino=new I2C(sda,scl);
00008     init();
00009 }
00010 Motorino* Motorino::gibMotorino()
00011     {
00012         printf("gibMotorino\r\n");
00013         if (meinMotorino==NULL)
00014             {
00015              meinMotorino=new Motorino(SDA,SCL);
00016             }
00017         return meinMotorino;   
00018         
00019     }
00020 char Motorino::getLED_ON_L(int nr)
00021 {
00022     return LED0_ON_L+4*nr;
00023 }
00024 
00025 char Motorino::getLED_ON_H(int nr)
00026 {
00027     return LED0_ON_L+4*nr+1;
00028 }
00029 
00030 char Motorino::getLED_OFF_L(int nr)
00031 {
00032     return LED0_ON_L+4*nr+2;
00033 }
00034 
00035 char Motorino::getLED_OFF_H(int nr)
00036 {
00037     return LED0_ON_L+4*nr+3;
00038 }
00039 
00040 void Motorino::WriteToASpecificRegister(char RegAdr,char Wert)
00041 {
00042     char daten[2];
00043     daten[0]=RegAdr;
00044     daten[1]=Wert;
00045     motorino->write(adresse,daten, 2, 0);
00046 }
00047 
00048 void Motorino::setDutyCycle(float dtc, int lednr)
00049 {
00050     unsigned int on,off;
00051     unsigned char onL,offL,onH,offH;
00052     on=0;
00053     off=4095*dtc/100;
00054     onL=on&0xFF;
00055     onH=on>>8&0xf;
00056     offL=off&0xFF;
00057     offH=off>>8&0xf;
00058     WriteToASpecificRegister(getLED_ON_L(lednr),onL);
00059     WriteToASpecificRegister(getLED_ON_H(lednr),onH);
00060     WriteToASpecificRegister(getLED_OFF_L(lednr),offL);
00061     WriteToASpecificRegister(getLED_OFF_H(lednr),offH);
00062 }
00063 
00064 void Motorino::init()
00065 {
00066     printf("Motorino Init\r\n");
00067     WriteToASpecificRegister(MODE1,SLEEP);
00068     WriteToASpecificRegister(PRE_SCALE,121); //50Hz Wert=Runden(25MHz/4096/50)-1
00069     WriteToASpecificRegister(MODE1,EXTCLK); //Switch to external 25MHz Clock
00070 
00071     setDutyCycle(25,0);
00072     
00073 }
00074 
00075 void Motorino::servo(float pos,int nr,float start, float end)  //pos 0..180°
00076 { 
00077     while(busy);
00078     busy=true;
00079     setDutyCycle(start+(end-start)*pos/180.0,nr);  
00080     busy=false;
00081 }