Alvaro Ferrán Cifuentes / Mbed 2 deprecated IntelliServo

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@0:200017bf1454, 2016-03-09 (annotated)

Committer:
alvaroferran
Date:
Wed Mar 09 15:58:24 2016 +0000
Revision:
0:200017bf1454
Proyect published

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alvaroferran 0:200017bf1454 1 //--------------------------------------------------------------
alvaroferran 0:200017bf1454 2 //-- IntelliServo
alvaroferran 0:200017bf1454 3 //-- I2C servo with temperature and current sensors
alvaroferran 0:200017bf1454 4 //--------------------------------------------------------------
alvaroferran 0:200017bf1454 5 //-- BQ
alvaroferran 0:200017bf1454 6 //--------------------------------------------------------------
alvaroferran 0:200017bf1454 7 //-- Firmware created by
alvaroferran 0:200017bf1454 8 //-- Alvaro Ferran Cifuentes (alvaroferran)
alvaroferran 0:200017bf1454 9 //--------------------------------------------------------------
alvaroferran 0:200017bf1454 10 //-- Released on March 2016
alvaroferran 0:200017bf1454 11 //-- under the GPL v3
alvaroferran 0:200017bf1454 12 //--------------------------------------------------------------
alvaroferran 0:200017bf1454 13
alvaroferran 0:200017bf1454 14 //Select servo
alvaroferran 0:200017bf1454 15 //#define FUTABA_S3003
alvaroferran 0:200017bf1454 16 #define TURNIGY_1268HV
alvaroferran 0:200017bf1454 17
alvaroferran 0:200017bf1454 18
alvaroferran 0:200017bf1454 19
alvaroferran 0:200017bf1454 20 #include "mbed.h"
alvaroferran 0:200017bf1454 21 #include "USBSerial.h"
alvaroferran 0:200017bf1454 22 #include "IAP.h"
alvaroferran 0:200017bf1454 23 #include "IntelliServoConfig.h"
alvaroferran 0:200017bf1454 24 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
alvaroferran 0:200017bf1454 25
alvaroferran 0:200017bf1454 26 //EEPROM values
alvaroferran 0:200017bf1454 27 #define TARGET_ADDRESS 64 //First non-reserved address in EEPROM
alvaroferran 0:200017bf1454 28 #define BYTE_SIZE 1
alvaroferran 0:200017bf1454 29
alvaroferran 0:200017bf1454 30 //USBSerial pc;
alvaroferran 0:200017bf1454 31 IAP iap;
alvaroferran 0:200017bf1454 32
alvaroferran 0:200017bf1454 33 I2CSlave slave(P0_5, P0_4); //SDA, SCL
alvaroferran 0:200017bf1454 34
alvaroferran 0:200017bf1454 35 PwmOut mot1(P0_8);
alvaroferran 0:200017bf1454 36 PwmOut mot2(P0_9);
alvaroferran 0:200017bf1454 37 AnalogIn pot(P0_11);
alvaroferran 0:200017bf1454 38 AnalogIn current(P0_13);
alvaroferran 0:200017bf1454 39 AnalogIn temperature(P0_14);
alvaroferran 0:200017bf1454 40
alvaroferran 0:200017bf1454 41 void setAngle(int16_t);
alvaroferran 0:200017bf1454 42 int16_t getAngle();
alvaroferran 0:200017bf1454 43 int16_t getCurrent();
alvaroferran 0:200017bf1454 44 int16_t getTemp();
alvaroferran 0:200017bf1454 45 void changeAddress(uint8_t);
alvaroferran 0:200017bf1454 46 void setEepromByte(int, uint8_t);
alvaroferran 0:200017bf1454 47 uint8_t getEepromByte(int);
alvaroferran 0:200017bf1454 48 int eepromAddres=1; //After TARGET_ADDRESS
alvaroferran 0:200017bf1454 49
alvaroferran 0:200017bf1454 50
alvaroferran 0:200017bf1454 51
alvaroferran 0:200017bf1454 52 /********MAIN***************************************************************/
alvaroferran 0:200017bf1454 53
alvaroferran 0:200017bf1454 54 int main() {
alvaroferran 0:200017bf1454 55
alvaroferran 0:200017bf1454 56 char inI2C[10];
alvaroferran 0:200017bf1454 57 char outI2C[2];
alvaroferran 0:200017bf1454 58 int8_t slaveRegister;
alvaroferran 0:200017bf1454 59 int16_t angleIn, angleOut, currentOut, tempOut;
alvaroferran 0:200017bf1454 60 bool angleSet=false;
alvaroferran 0:200017bf1454 61 int8_t disable=1;
alvaroferran 0:200017bf1454 62
alvaroferran 0:200017bf1454 63 mot2.period_us(33.33f); // 30kHz
alvaroferran 0:200017bf1454 64 mot1.period_us(33.33f);
alvaroferran 0:200017bf1454 65 mot1.write(0.00f);
alvaroferran 0:200017bf1454 66 mot2.write(0.00f);
alvaroferran 0:200017bf1454 67
alvaroferran 0:200017bf1454 68 //Uncomment to reset default i2c address
alvaroferran 0:200017bf1454 69 //setEepromByte(PLACE,(uint8_t) 0x01);
alvaroferran 0:200017bf1454 70
alvaroferran 0:200017bf1454 71 uint8_t inValue, i2cAddress=0x02;
alvaroferran 0:200017bf1454 72 inValue=getEepromByte(eepromAddres);
alvaroferran 0:200017bf1454 73 if(inValue!= (uint8_t) 0 ) i2cAddress=inValue;
alvaroferran 0:200017bf1454 74 slave.address(i2cAddress<<1);
alvaroferran 0:200017bf1454 75
alvaroferran 0:200017bf1454 76
alvaroferran 0:200017bf1454 77 while(1){
alvaroferran 0:200017bf1454 78
alvaroferran 0:200017bf1454 79 //setAngle(90);
alvaroferran 0:200017bf1454 80
alvaroferran 0:200017bf1454 81
alvaroferran 0:200017bf1454 82 int i = slave.receive();
alvaroferran 0:200017bf1454 83 switch (i) {
alvaroferran 0:200017bf1454 84
alvaroferran 0:200017bf1454 85 case I2CSlave::WriteAddressed: //Receive
alvaroferran 0:200017bf1454 86
alvaroferran 0:200017bf1454 87 slave.read(inI2C, 10);
alvaroferran 0:200017bf1454 88 slaveRegister=inI2C[0];
alvaroferran 0:200017bf1454 89
alvaroferran 0:200017bf1454 90 switch(slaveRegister){
alvaroferran 0:200017bf1454 91 case 0x0A: //Change address
alvaroferran 0:200017bf1454 92 changeAddress(inI2C[1]);
alvaroferran 0:200017bf1454 93 break;
alvaroferran 0:200017bf1454 94
alvaroferran 0:200017bf1454 95 case 0x01: //Disable motor
alvaroferran 0:200017bf1454 96 disable=inI2C[1];
alvaroferran 0:200017bf1454 97 break;
alvaroferran 0:200017bf1454 98
alvaroferran 0:200017bf1454 99 case 0x02: //Set angle
alvaroferran 0:200017bf1454 100 angleIn=(int16_t) (inI2C[1] << 8) | inI2C[2];
alvaroferran 0:200017bf1454 101 disable=0;
alvaroferran 0:200017bf1454 102 angleSet=true;
alvaroferran 0:200017bf1454 103 break;
alvaroferran 0:200017bf1454 104 }
alvaroferran 0:200017bf1454 105
alvaroferran 0:200017bf1454 106
alvaroferran 0:200017bf1454 107 break;
alvaroferran 0:200017bf1454 108
alvaroferran 0:200017bf1454 109
alvaroferran 0:200017bf1454 110 case I2CSlave::ReadAddressed: //Reply
alvaroferran 0:200017bf1454 111
alvaroferran 0:200017bf1454 112 switch(slaveRegister){
alvaroferran 0:200017bf1454 113 case 0x04: //Get angle
alvaroferran 0:200017bf1454 114 outI2C[0]= (angleOut >> 8) & 0xFF;
alvaroferran 0:200017bf1454 115 outI2C[1]= angleOut & 0xFF;
alvaroferran 0:200017bf1454 116 break;
alvaroferran 0:200017bf1454 117
alvaroferran 0:200017bf1454 118 case 0x06: //Get current
alvaroferran 0:200017bf1454 119 outI2C[0]= (currentOut >> 8) & 0xFF;
alvaroferran 0:200017bf1454 120 outI2C[1]= currentOut & 0xFF;
alvaroferran 0:200017bf1454 121 break;
alvaroferran 0:200017bf1454 122
alvaroferran 0:200017bf1454 123 case 0x08: //Get temperature
alvaroferran 0:200017bf1454 124 outI2C[0]= (tempOut >> 8) & 0xFF;
alvaroferran 0:200017bf1454 125 outI2C[1]= tempOut & 0xFF;
alvaroferran 0:200017bf1454 126 break;
alvaroferran 0:200017bf1454 127 }
alvaroferran 0:200017bf1454 128
alvaroferran 0:200017bf1454 129 slave.write(outI2C, 2);
alvaroferran 0:200017bf1454 130
alvaroferran 0:200017bf1454 131 break;
alvaroferran 0:200017bf1454 132
alvaroferran 0:200017bf1454 133 }
alvaroferran 0:200017bf1454 134 for(int i = 0; i < 10; i++) inI2C[i] = 0;
alvaroferran 0:200017bf1454 135
alvaroferran 0:200017bf1454 136
alvaroferran 0:200017bf1454 137
alvaroferran 0:200017bf1454 138 //Keep setting the angle so the servo stays energized
alvaroferran 0:200017bf1454 139 if(angleSet==true){ //Make sure servo only starts moving when a value has been sent
alvaroferran 0:200017bf1454 140 if(disable==0)
alvaroferran 0:200017bf1454 141 setAngle(angleIn);
alvaroferran 0:200017bf1454 142 else {
alvaroferran 0:200017bf1454 143 mot1.write(0.00f);
alvaroferran 0:200017bf1454 144 mot2.write(0.00f);
alvaroferran 0:200017bf1454 145 }
alvaroferran 0:200017bf1454 146 }
alvaroferran 0:200017bf1454 147
alvaroferran 0:200017bf1454 148 angleOut=getAngle();
alvaroferran 0:200017bf1454 149 currentOut=getCurrent();
alvaroferran 0:200017bf1454 150 tempOut=getTemp();
alvaroferran 0:200017bf1454 151
alvaroferran 0:200017bf1454 152
alvaroferran 0:200017bf1454 153 }
alvaroferran 0:200017bf1454 154
alvaroferran 0:200017bf1454 155 }
alvaroferran 0:200017bf1454 156
alvaroferran 0:200017bf1454 157
alvaroferran 0:200017bf1454 158
alvaroferran 0:200017bf1454 159
alvaroferran 0:200017bf1454 160
alvaroferran 0:200017bf1454 161
alvaroferran 0:200017bf1454 162 /********SET ANGLE**********************************************************/
alvaroferran 0:200017bf1454 163 //PID setting based on Angel Espeso's example (http://roble.uno/control-pid-barra-y-bola-arduino/)
alvaroferran 0:200017bf1454 164
alvaroferran 0:200017bf1454 165 void setAngle(int16_t desiredAngle){
alvaroferran 0:200017bf1454 166
alvaroferran 0:200017bf1454 167 desiredAngle=constrain(desiredAngle,-1,maxAngle); //Limit legal angles
alvaroferran 0:200017bf1454 168
alvaroferran 0:200017bf1454 169 int16_t currentAngle=0, lastAngle, error=0, lastError;
alvaroferran 0:200017bf1454 170 int eCount=0;
alvaroferran 0:200017bf1454 171 float I=0;
alvaroferran 0:200017bf1454 172 while(1){
alvaroferran 0:200017bf1454 173
alvaroferran 0:200017bf1454 174 wait_ms(1);
alvaroferran 0:200017bf1454 175
alvaroferran 0:200017bf1454 176 //Read current angle
alvaroferran 0:200017bf1454 177 lastAngle=currentAngle;
alvaroferran 0:200017bf1454 178 currentAngle=getAngle();
alvaroferran 0:200017bf1454 179
alvaroferran 0:200017bf1454 180 //Error
alvaroferran 0:200017bf1454 181 lastError=error;
alvaroferran 0:200017bf1454 182 error=desiredAngle-currentAngle;
alvaroferran 0:200017bf1454 183
alvaroferran 0:200017bf1454 184 //Calculate average speed
alvaroferran 0:200017bf1454 185 float v[5]; //Speed vector
alvaroferran 0:200017bf1454 186 for (int i=0; i<5; i++) // Move all speed values one space to the left to make space for the newest one
alvaroferran 0:200017bf1454 187 v[i] =v [i+1];
alvaroferran 0:200017bf1454 188 v[4] = (error-lastError); // Add last speed
alvaroferran 0:200017bf1454 189 float vel=0;
alvaroferran 0:200017bf1454 190 for (int i=0; i<5; i++) // Average speed
alvaroferran 0:200017bf1454 191 vel += v[i];
alvaroferran 0:200017bf1454 192 vel /= 5;
alvaroferran 0:200017bf1454 193 vel/=100;
alvaroferran 0:200017bf1454 194
alvaroferran 0:200017bf1454 195 //I
alvaroferran 0:200017bf1454 196 if(abs(error)<10 && abs(error)>0.2)
alvaroferran 0:200017bf1454 197 I+=error*Ki;
alvaroferran 0:200017bf1454 198 else
alvaroferran 0:200017bf1454 199 I=0;
alvaroferran 0:200017bf1454 200
alvaroferran 0:200017bf1454 201 //Voltage calculation
alvaroferran 0:200017bf1454 202 float pwr=Kp*error+Kd*vel+I;
alvaroferran 0:200017bf1454 203
alvaroferran 0:200017bf1454 204 //Motor control
alvaroferran 0:200017bf1454 205 float speed=0;//0.06;
alvaroferran 0:200017bf1454 206 if(pwr>0){
alvaroferran 0:200017bf1454 207 mot1.write(1-(abs(pwr)+speed));
alvaroferran 0:200017bf1454 208 mot2.write(1);
alvaroferran 0:200017bf1454 209 }else {
alvaroferran 0:200017bf1454 210 mot2.write(1-(abs(pwr)+speed));
alvaroferran 0:200017bf1454 211 mot1.write(1);
alvaroferran 0:200017bf1454 212 }
alvaroferran 0:200017bf1454 213
alvaroferran 0:200017bf1454 214 //pc.printf("Pot Angle: %d Pwr: %.2f Current: %.2f\n" ,currentAngle ,abs(pwr)+speed ,current.read()*100/0.055);
alvaroferran 0:200017bf1454 215
alvaroferran 0:200017bf1454 216 //Angle stable
alvaroferran 0:200017bf1454 217 if(abs(currentAngle-lastAngle)<=1){ //Difference in angle instead of error to avoid blocking when angle is not recheable
alvaroferran 0:200017bf1454 218 eCount++;
alvaroferran 0:200017bf1454 219 if(eCount>5)
alvaroferran 0:200017bf1454 220 break;
alvaroferran 0:200017bf1454 221 }
alvaroferran 0:200017bf1454 222
alvaroferran 0:200017bf1454 223
alvaroferran 0:200017bf1454 224 }
alvaroferran 0:200017bf1454 225 }
alvaroferran 0:200017bf1454 226
alvaroferran 0:200017bf1454 227
alvaroferran 0:200017bf1454 228
alvaroferran 0:200017bf1454 229 /********GET ANGLE**********************************************************/
alvaroferran 0:200017bf1454 230
alvaroferran 0:200017bf1454 231 int16_t getAngle(){
alvaroferran 0:200017bf1454 232 float aF=0;
alvaroferran 0:200017bf1454 233 int16_t aI=0;
alvaroferran 0:200017bf1454 234 for(int i=0; i<5; i++)
alvaroferran 0:200017bf1454 235 aF+=(pot.read()*100-5)*2.439;
alvaroferran 0:200017bf1454 236 aI=(int16_t)constrain(aF/5,0,maxAngle);
alvaroferran 0:200017bf1454 237 return aI;
alvaroferran 0:200017bf1454 238 }
alvaroferran 0:200017bf1454 239
alvaroferran 0:200017bf1454 240
alvaroferran 0:200017bf1454 241
alvaroferran 0:200017bf1454 242 /********GET CURRENT*********************************************************/
alvaroferran 0:200017bf1454 243
alvaroferran 0:200017bf1454 244 int16_t getCurrent(){
alvaroferran 0:200017bf1454 245 float cF=0;
alvaroferran 0:200017bf1454 246 int16_t cI=0;
alvaroferran 0:200017bf1454 247 for(int i=0; i<5; i++)
alvaroferran 0:200017bf1454 248 cF+=current.read()*100/0.055;
alvaroferran 0:200017bf1454 249 cI=(int16_t)cF/5;
alvaroferran 0:200017bf1454 250 return cI;
alvaroferran 0:200017bf1454 251 }
alvaroferran 0:200017bf1454 252
alvaroferran 0:200017bf1454 253
alvaroferran 0:200017bf1454 254
alvaroferran 0:200017bf1454 255
alvaroferran 0:200017bf1454 256
alvaroferran 0:200017bf1454 257
alvaroferran 0:200017bf1454 258 /********GET TEMPERATURE****************************************************/
alvaroferran 0:200017bf1454 259
alvaroferran 0:200017bf1454 260 int16_t getTemp(){
alvaroferran 0:200017bf1454 261 return (int16_t) (temperature.read()*3300-500)/10;
alvaroferran 0:200017bf1454 262 }
alvaroferran 0:200017bf1454 263
alvaroferran 0:200017bf1454 264
alvaroferran 0:200017bf1454 265
alvaroferran 0:200017bf1454 266
alvaroferran 0:200017bf1454 267 /********CHANGE ADDRESS*****************************************************/
alvaroferran 0:200017bf1454 268
alvaroferran 0:200017bf1454 269 void changeAddress(uint8_t address){
alvaroferran 0:200017bf1454 270 setEepromByte(eepromAddres, address);
alvaroferran 0:200017bf1454 271 slave.address(address<<1);
alvaroferran 0:200017bf1454 272 }
alvaroferran 0:200017bf1454 273
alvaroferran 0:200017bf1454 274
alvaroferran 0:200017bf1454 275 /********EEPROM R&W*********************************************************/
alvaroferran 0:200017bf1454 276 //EEPROM acces code based on Dave Tech's example (https://developer.mbed.org/users/kstech/code/EepromTest/)
alvaroferran 0:200017bf1454 277
alvaroferran 0:200017bf1454 278 typedef union data {
alvaroferran 0:200017bf1454 279 uint8_t b;
alvaroferran 0:200017bf1454 280 char s[1];
alvaroferran 0:200017bf1454 281 } myData;
alvaroferran 0:200017bf1454 282
alvaroferran 0:200017bf1454 283
alvaroferran 0:200017bf1454 284 void setEepromByte(int place, uint8_t incomingByte){
alvaroferran 0:200017bf1454 285 myData setByte;
alvaroferran 0:200017bf1454 286 char someBytes[1];
alvaroferran 0:200017bf1454 287 setByte.b = incomingByte;
alvaroferran 0:200017bf1454 288 for(int i=0; i<sizeof(someBytes); i++)
alvaroferran 0:200017bf1454 289 someBytes[i]=setByte.s[i];
alvaroferran 0:200017bf1454 290 iap.write_eeprom( someBytes, (char*) (TARGET_ADDRESS+place), BYTE_SIZE );
alvaroferran 0:200017bf1454 291 }
alvaroferran 0:200017bf1454 292
alvaroferran 0:200017bf1454 293 uint8_t getEepromByte(int place){
alvaroferran 0:200017bf1454 294 myData getByte;
alvaroferran 0:200017bf1454 295 char someBytes[1];
alvaroferran 0:200017bf1454 296 iap.read_eeprom( (char*)(TARGET_ADDRESS+place), someBytes, BYTE_SIZE );
alvaroferran 0:200017bf1454 297 for(int i=0; i<sizeof(someBytes); i++)
alvaroferran 0:200017bf1454 298 getByte.s[i]=someBytes[i];
alvaroferran 0:200017bf1454 299 return getByte.b;
alvaroferran 0:200017bf1454 300 }