Alvaro Ferrán Cifuentes / Mbed 2 deprecated IntelliServo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   IAP USBDevice mbed

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 }