IntelliServo is a project aiming to transform regular hobby servos into intelligent ones by replacing their original boards. By doing so the servo has upgraded capabilities, such as being able to read its position, temperature and current consumption. Multiple servos can now be daisy-chained and controlled over I2C from a microcontroller, or over USB from a computer. https://github.com/bqlabs/IntelliServo
Dependencies: IAP USBDevice mbed
All the files and info are available in Github (http://alvaroferran.github.io/IntelliServo), including the board design, arduino library, servo firmware and mechanical parts as well as instructions for use
IntelliServoMain.cpp
- Committer:
- alvaroferran
- Date:
- 2016-03-09
- Revision:
- 0:200017bf1454
File content as of revision 0:200017bf1454:
//-------------------------------------------------------------- //-- IntelliServo //-- I2C servo with temperature and current sensors //-------------------------------------------------------------- //-- BQ //-------------------------------------------------------------- //-- Firmware created by //-- Alvaro Ferran Cifuentes (alvaroferran) //-------------------------------------------------------------- //-- Released on March 2016 //-- under the GPL v3 //-------------------------------------------------------------- //Select servo //#define FUTABA_S3003 #define TURNIGY_1268HV #include "mbed.h" #include "USBSerial.h" #include "IAP.h" #include "IntelliServoConfig.h" #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //EEPROM values #define TARGET_ADDRESS 64 //First non-reserved address in EEPROM #define BYTE_SIZE 1 //USBSerial pc; IAP iap; I2CSlave slave(P0_5, P0_4); //SDA, SCL PwmOut mot1(P0_8); PwmOut mot2(P0_9); AnalogIn pot(P0_11); AnalogIn current(P0_13); AnalogIn temperature(P0_14); void setAngle(int16_t); int16_t getAngle(); int16_t getCurrent(); int16_t getTemp(); void changeAddress(uint8_t); void setEepromByte(int, uint8_t); uint8_t getEepromByte(int); int eepromAddres=1; //After TARGET_ADDRESS /********MAIN***************************************************************/ int main() { char inI2C[10]; char outI2C[2]; int8_t slaveRegister; int16_t angleIn, angleOut, currentOut, tempOut; bool angleSet=false; int8_t disable=1; mot2.period_us(33.33f); // 30kHz mot1.period_us(33.33f); mot1.write(0.00f); mot2.write(0.00f); //Uncomment to reset default i2c address //setEepromByte(PLACE,(uint8_t) 0x01); uint8_t inValue, i2cAddress=0x02; inValue=getEepromByte(eepromAddres); if(inValue!= (uint8_t) 0 ) i2cAddress=inValue; slave.address(i2cAddress<<1); while(1){ //setAngle(90); int i = slave.receive(); switch (i) { case I2CSlave::WriteAddressed: //Receive slave.read(inI2C, 10); slaveRegister=inI2C[0]; switch(slaveRegister){ case 0x0A: //Change address changeAddress(inI2C[1]); break; case 0x01: //Disable motor disable=inI2C[1]; break; case 0x02: //Set angle angleIn=(int16_t) (inI2C[1] << 8) | inI2C[2]; disable=0; angleSet=true; break; } break; case I2CSlave::ReadAddressed: //Reply switch(slaveRegister){ case 0x04: //Get angle outI2C[0]= (angleOut >> 8) & 0xFF; outI2C[1]= angleOut & 0xFF; break; case 0x06: //Get current outI2C[0]= (currentOut >> 8) & 0xFF; outI2C[1]= currentOut & 0xFF; break; case 0x08: //Get temperature outI2C[0]= (tempOut >> 8) & 0xFF; outI2C[1]= tempOut & 0xFF; break; } slave.write(outI2C, 2); break; } for(int i = 0; i < 10; i++) inI2C[i] = 0; //Keep setting the angle so the servo stays energized if(angleSet==true){ //Make sure servo only starts moving when a value has been sent if(disable==0) setAngle(angleIn); else { mot1.write(0.00f); mot2.write(0.00f); } } angleOut=getAngle(); currentOut=getCurrent(); tempOut=getTemp(); } } /********SET ANGLE**********************************************************/ //PID setting based on Angel Espeso's example (http://roble.uno/control-pid-barra-y-bola-arduino/) void setAngle(int16_t desiredAngle){ desiredAngle=constrain(desiredAngle,-1,maxAngle); //Limit legal angles int16_t currentAngle=0, lastAngle, error=0, lastError; int eCount=0; float I=0; while(1){ wait_ms(1); //Read current angle lastAngle=currentAngle; currentAngle=getAngle(); //Error lastError=error; error=desiredAngle-currentAngle; //Calculate average speed float v[5]; //Speed vector for (int i=0; i<5; i++) // Move all speed values one space to the left to make space for the newest one v[i] =v [i+1]; v[4] = (error-lastError); // Add last speed float vel=0; for (int i=0; i<5; i++) // Average speed vel += v[i]; vel /= 5; vel/=100; //I if(abs(error)<10 && abs(error)>0.2) I+=error*Ki; else I=0; //Voltage calculation float pwr=Kp*error+Kd*vel+I; //Motor control float speed=0;//0.06; if(pwr>0){ mot1.write(1-(abs(pwr)+speed)); mot2.write(1); }else { mot2.write(1-(abs(pwr)+speed)); mot1.write(1); } //pc.printf("Pot Angle: %d Pwr: %.2f Current: %.2f\n" ,currentAngle ,abs(pwr)+speed ,current.read()*100/0.055); //Angle stable if(abs(currentAngle-lastAngle)<=1){ //Difference in angle instead of error to avoid blocking when angle is not recheable eCount++; if(eCount>5) break; } } } /********GET ANGLE**********************************************************/ int16_t getAngle(){ float aF=0; int16_t aI=0; for(int i=0; i<5; i++) aF+=(pot.read()*100-5)*2.439; aI=(int16_t)constrain(aF/5,0,maxAngle); return aI; } /********GET CURRENT*********************************************************/ int16_t getCurrent(){ float cF=0; int16_t cI=0; for(int i=0; i<5; i++) cF+=current.read()*100/0.055; cI=(int16_t)cF/5; return cI; } /********GET TEMPERATURE****************************************************/ int16_t getTemp(){ return (int16_t) (temperature.read()*3300-500)/10; } /********CHANGE ADDRESS*****************************************************/ void changeAddress(uint8_t address){ setEepromByte(eepromAddres, address); slave.address(address<<1); } /********EEPROM R&W*********************************************************/ //EEPROM acces code based on Dave Tech's example (https://developer.mbed.org/users/kstech/code/EepromTest/) typedef union data { uint8_t b; char s[1]; } myData; void setEepromByte(int place, uint8_t incomingByte){ myData setByte; char someBytes[1]; setByte.b = incomingByte; for(int i=0; i<sizeof(someBytes); i++) someBytes[i]=setByte.s[i]; iap.write_eeprom( someBytes, (char*) (TARGET_ADDRESS+place), BYTE_SIZE ); } uint8_t getEepromByte(int place){ myData getByte; char someBytes[1]; iap.read_eeprom( (char*)(TARGET_ADDRESS+place), someBytes, BYTE_SIZE ); for(int i=0; i<sizeof(someBytes); i++) getByte.s[i]=someBytes[i]; return getByte.b; }