IntelliServoMain.cpp

00001 //--------------------------------------------------------------
00002 //--    IntelliServo
00003 //--    I2C servo with temperature and current sensors
00004 //--------------------------------------------------------------
00005 //--    BQ
00006 //--------------------------------------------------------------
00007 //--    Firmware created by 
00008 //--        Alvaro Ferran Cifuentes (alvaroferran)
00009 //--------------------------------------------------------------
00010 //--    Released on March 2016
00011 //--    under the GPL v3
00012 //--------------------------------------------------------------
00013 
00014 //Select servo
00015 //#define FUTABA_S3003
00016 #define TURNIGY_1268HV
00017 
00018 
00019 
00020 #include "mbed.h"
00021 #include "USBSerial.h"
00022 #include "IAP.h"
00023 #include "IntelliServoConfig.h"
00024 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
00025 
00026 //EEPROM values
00027 #define     TARGET_ADDRESS  64 //First non-reserved address in EEPROM
00028 #define     BYTE_SIZE       1
00029 
00030 //USBSerial pc;
00031 IAP     iap;
00032 
00033 I2CSlave slave(P0_5, P0_4); //SDA, SCL
00034 
00035 PwmOut mot1(P0_8);
00036 PwmOut mot2(P0_9);
00037 AnalogIn   pot(P0_11);
00038 AnalogIn   current(P0_13);
00039 AnalogIn   temperature(P0_14); 
00040 
00041 void setAngle(int16_t);
00042 int16_t getAngle();
00043 int16_t getCurrent();
00044 int16_t getTemp();
00045 void changeAddress(uint8_t);
00046 void setEepromByte(int, uint8_t);
00047 uint8_t getEepromByte(int);
00048 int eepromAddres=1; //After TARGET_ADDRESS
00049 
00050 
00051 
00052 /********MAIN***************************************************************/
00053 
00054 int main() {
00055     
00056     char inI2C[10];
00057     char outI2C[2];
00058     int8_t slaveRegister;
00059     int16_t angleIn, angleOut, currentOut, tempOut;
00060     bool angleSet=false;
00061     int8_t disable=1;
00062     
00063     mot2.period_us(33.33f);  // 30kHz 
00064     mot1.period_us(33.33f);  
00065     mot1.write(0.00f);
00066     mot2.write(0.00f);
00067 
00068     //Uncomment to reset default i2c address
00069     //setEepromByte(PLACE,(uint8_t) 0x01);
00070 
00071     uint8_t inValue, i2cAddress=0x02;
00072     inValue=getEepromByte(eepromAddres);
00073     if(inValue!= (uint8_t) 0 ) i2cAddress=inValue; 
00074     slave.address(i2cAddress<<1);    
00075 
00076 
00077     while(1){
00078              
00079         //setAngle(90);
00080       
00081              
00082         int i = slave.receive();
00083         switch (i) {           
00084             
00085             case I2CSlave::WriteAddressed:      //Receive
00086                 
00087                 slave.read(inI2C, 10);
00088                 slaveRegister=inI2C[0];
00089 
00090                 switch(slaveRegister){
00091                     case 0x0A:  //Change address
00092                         changeAddress(inI2C[1]);
00093                     break;
00094                     
00095                     case 0x01:  //Disable motor
00096                         disable=inI2C[1];
00097                     break;
00098                     
00099                     case 0x02:  //Set angle
00100                         angleIn=(int16_t)  (inI2C[1] << 8) | inI2C[2];
00101                         disable=0; 
00102                         angleSet=true;
00103                     break;
00104                 }
00105                 
00106                 
00107             break;
00108             
00109             
00110             case I2CSlave::ReadAddressed:       //Reply
00111                                
00112                 switch(slaveRegister){
00113                     case 0x04:  //Get angle
00114                         outI2C[0]= (angleOut >> 8) & 0xFF;
00115                         outI2C[1]= angleOut & 0xFF;
00116                     break;
00117                     
00118                     case 0x06:  //Get current
00119                         outI2C[0]= (currentOut >> 8) & 0xFF;
00120                         outI2C[1]= currentOut & 0xFF;
00121                     break;
00122                     
00123                     case 0x08:  //Get temperature
00124                         outI2C[0]= (tempOut >> 8) & 0xFF;
00125                         outI2C[1]= tempOut & 0xFF;
00126                     break;
00127                 }
00128                 
00129                 slave.write(outI2C, 2); 
00130                 
00131             break;
00132             
00133         }
00134         for(int i = 0; i < 10; i++) inI2C[i] = 0;  
00135         
00136        
00137         
00138        //Keep setting the angle so the servo stays energized
00139        if(angleSet==true){ //Make sure servo only starts moving when a value has been sent
00140             if(disable==0)
00141                 setAngle(angleIn);
00142             else {
00143                 mot1.write(0.00f);
00144                 mot2.write(0.00f);
00145             }
00146        }
00147         
00148        angleOut=getAngle();
00149        currentOut=getCurrent();
00150        tempOut=getTemp();
00151     
00152       
00153     }
00154     
00155 }
00156 
00157 
00158 
00159 
00160 
00161 
00162 /********SET ANGLE**********************************************************/
00163 //PID setting based on Angel Espeso's example (http://roble.uno/control-pid-barra-y-bola-arduino/)
00164 
00165 void setAngle(int16_t desiredAngle){
00166 
00167     desiredAngle=constrain(desiredAngle,-1,maxAngle);  //Limit legal angles
00168     
00169     int16_t currentAngle=0, lastAngle, error=0, lastError;
00170     int eCount=0;
00171     float I=0;
00172     while(1){
00173         
00174         wait_ms(1);
00175         
00176         //Read current angle 
00177         lastAngle=currentAngle;
00178         currentAngle=getAngle();
00179         
00180         //Error
00181         lastError=error;
00182         error=desiredAngle-currentAngle;
00183         
00184         //Calculate average speed
00185         float v[5];   //Speed vector
00186         for (int i=0; i<5; i++) // Move all speed values one space to the left to make space for the newest one
00187             v[i] =v [i+1];
00188         v[4] = (error-lastError); // Add last speed
00189         float vel=0;
00190         for (int i=0; i<5; i++)     // Average speed
00191             vel += v[i];
00192         vel /= 5;
00193         vel/=100;
00194                 
00195         //I 
00196         if(abs(error)<10 && abs(error)>0.2)
00197             I+=error*Ki;
00198         else 
00199             I=0;
00200         
00201         //Voltage calculation
00202         float pwr=Kp*error+Kd*vel+I;
00203         
00204         //Motor control
00205         float speed=0;//0.06; 
00206         if(pwr>0){
00207             mot1.write(1-(abs(pwr)+speed)); 
00208             mot2.write(1); 
00209         }else {
00210             mot2.write(1-(abs(pwr)+speed)); 
00211             mot1.write(1);
00212         }
00213         
00214         //pc.printf("Pot Angle: %d   Pwr: %.2f  Current: %.2f\n" ,currentAngle ,abs(pwr)+speed ,current.read()*100/0.055);
00215     
00216         //Angle stable
00217         if(abs(currentAngle-lastAngle)<=1){     //Difference in angle instead of error to avoid blocking when angle is not recheable
00218             eCount++;
00219             if(eCount>5) 
00220                 break;
00221         }
00222         
00223         
00224     }
00225 }
00226 
00227 
00228 
00229 /********GET ANGLE**********************************************************/
00230 
00231 int16_t getAngle(){
00232     float aF=0;
00233     int16_t aI=0;
00234     for(int i=0; i<5; i++)
00235          aF+=(pot.read()*100-5)*2.439; 
00236     aI=(int16_t)constrain(aF/5,0,maxAngle);
00237     return aI;
00238 }
00239 
00240 
00241 
00242 /********GET CURRENT*********************************************************/
00243 
00244 int16_t getCurrent(){
00245     float cF=0;
00246     int16_t cI=0;
00247     for(int i=0; i<5; i++)
00248          cF+=current.read()*100/0.055; 
00249     cI=(int16_t)cF/5;
00250     return cI;
00251 }
00252 
00253 
00254 
00255 
00256 
00257 
00258 /********GET TEMPERATURE****************************************************/
00259 
00260 int16_t getTemp(){ 
00261     return (int16_t) (temperature.read()*3300-500)/10;
00262 }
00263 
00264 
00265 
00266 
00267 /********CHANGE ADDRESS*****************************************************/
00268 
00269 void changeAddress(uint8_t address){
00270     setEepromByte(eepromAddres, address);
00271     slave.address(address<<1);  
00272 }
00273 
00274 
00275 /********EEPROM R&W*********************************************************/
00276 //EEPROM acces code based on Dave Tech's example (https://developer.mbed.org/users/kstech/code/EepromTest/)
00277 
00278 typedef union data {
00279     uint8_t b;
00280     char  s[1];
00281 } myData; 
00282 
00283   
00284 void setEepromByte(int place, uint8_t incomingByte){
00285     myData setByte;
00286     char someBytes[1];
00287     setByte.b = incomingByte;
00288     for(int i=0; i<sizeof(someBytes); i++)
00289         someBytes[i]=setByte.s[i];
00290     iap.write_eeprom( someBytes, (char*) (TARGET_ADDRESS+place), BYTE_SIZE );
00291 } 
00292  
00293 uint8_t getEepromByte(int place){
00294     myData getByte;
00295     char someBytes[1];
00296     iap.read_eeprom( (char*)(TARGET_ADDRESS+place), someBytes, BYTE_SIZE );
00297     for(int i=0; i<sizeof(someBytes); i++)
00298         getByte.s[i]=someBytes[i];
00299     return getByte.b;
00300 }