MorphGI / Mbed 2 deprecated MGI_Rebuild_LowLevel_v7_PressureCtrl_PWM

Pressure control for drive segment in rebuild of control unit.

Dependencies:   mbed QEI FastAnalogIn mbed-rtos FastPWM

main.cpp@3:9bd35e5b05ba, 2019-01-29 (annotated)

Committer:
dofydoink
Date:
Tue Jan 29 14:57:57 2019 +0000
Revision:
3:9bd35e5b05ba
Parent:
2:aee7d4724915
Child:
4:3bab17dfae4e
Latest Working version_safe so far

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dofydoink 0:20018747657d 1 #include "mbed.h"
dofydoink 0:20018747657d 2 #include "math.h"
dofydoink 0:20018747657d 3
dofydoink 0:20018747657d 4 #include "QEI.h"
dofydoink 0:20018747657d 5
dofydoink 0:20018747657d 6 #include "rtos.h"
dofydoink 0:20018747657d 7 #include "FastPWM.h"
dofydoink 0:20018747657d 8
dofydoink 0:20018747657d 9 #define ERROR_LIMIT_POS 0.008 //limits for when any error measurement is considered to be zero.
dofydoink 0:20018747657d 10 #define ERROR_LIMIT_NEG -0.008
dofydoink 0:20018747657d 11
dofydoink 0:20018747657d 12 #define CHAN_1 0x80
dofydoink 0:20018747657d 13 #define CHAN_2 0x90
dofydoink 0:20018747657d 14 #define CHAN_3 0xA0
dofydoink 0:20018747657d 15 #define CHAN_4 0xB0
dofydoink 0:20018747657d 16
dofydoink 0:20018747657d 17 #define PRESSURE_CHAN 1
dofydoink 0:20018747657d 18 #define POSITION_CHAN 3
dofydoink 0:20018747657d 19
dofydoink 0:20018747657d 20
dofydoink 0:20018747657d 21
dofydoink 3:9bd35e5b05ba 22 const double MAX_SPEED_MMPS = 24.3457;
dofydoink 2:aee7d4724915 23 int intDummy;
dofydoink 0:20018747657d 24 SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
dofydoink 0:20018747657d 25
dofydoink 0:20018747657d 26 QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);
dofydoink 0:20018747657d 27
dofydoink 0:20018747657d 28 DigitalOut Mntr(D3);
dofydoink 2:aee7d4724915 29 DigitalOut Mntr2(PB_8);
dofydoink 0:20018747657d 30
dofydoink 3:9bd35e5b05ba 31 #define EXTERNAL_CLOCK_MODE 0x08
dofydoink 3:9bd35e5b05ba 32 #define RANGE_CONFIG 0x03
dofydoink 0:20018747657d 33
dofydoink 0:20018747657d 34 #define EXTERNAL_CLOCK_MODE 0x08
dofydoink 0:20018747657d 35 #define RANGE_CONFIG 0x03 //config for 1.5*Vref = 6.144V
dofydoink 0:20018747657d 36
dofydoink 0:20018747657d 37 #define PRESSURE_BIAS_VOLTAGE 0.15151515151515
dofydoink 3:9bd35e5b05ba 38
dofydoink 3:9bd35e5b05ba 39 const double MAX_ACTUATOR_LENGTH = 52.2;
dofydoink 3:9bd35e5b05ba 40
dofydoink 3:9bd35e5b05ba 41 const double MAX_POSITION_MM = 40.0; //maximum actuator position position in mm
dofydoink 3:9bd35e5b05ba 42
dofydoink 3:9bd35e5b05ba 43 //sample time variables
dofydoink 3:9bd35e5b05ba 44 double dblSampleTime_s = 0.001;
dofydoink 0:20018747657d 45
dofydoink 0:20018747657d 46 #define POT_2_MM 0.006750412 //convert potentiometer reading to mm (Tested and is right)
dofydoink 0:20018747657d 47 #define POT_OFFSET 7500//6666
dofydoink 0:20018747657d 48
dofydoink 0:20018747657d 49 Serial pc(USBTX, USBRX); // tx, rx
dofydoink 0:20018747657d 50
dofydoink 0:20018747657d 51 Thread PositionControlThread(osPriorityHigh);
dofydoink 0:20018747657d 52 Thread DebugThread(osPriorityNormal);
dofydoink 0:20018747657d 53 Thread GateControlThread(osPriorityNormal);
dofydoink 0:20018747657d 54
dofydoink 0:20018747657d 55 Thread DutyCycleThread(osPriorityRealtime);
dofydoink 0:20018747657d 56 Mutex mutDutyCycle;
dofydoink 0:20018747657d 57 Semaphore semDutyCycle(1);
dofydoink 0:20018747657d 58 Timer timerDutyCycle;
dofydoink 0:20018747657d 59
dofydoink 0:20018747657d 60 volatile unsigned int intT;
dofydoink 0:20018747657d 61 volatile unsigned int intDeltaT;
dofydoink 0:20018747657d 62 double dblDutyCycle;
dofydoink 0:20018747657d 63
dofydoink 0:20018747657d 64 double dblSensorDriftError;
dofydoink 0:20018747657d 65
dofydoink 0:20018747657d 66 short randomvar;
dofydoink 0:20018747657d 67
dofydoink 0:20018747657d 68 volatile int dataRx;
dofydoink 0:20018747657d 69
dofydoink 0:20018747657d 70 void CalculateDutyCycle()
dofydoink 0:20018747657d 71 {
dofydoink 0:20018747657d 72 while(1)
dofydoink 0:20018747657d 73 {
dofydoink 0:20018747657d 74 semDutyCycle.wait();
dofydoink 0:20018747657d 75 mutDutyCycle.lock();
dofydoink 0:20018747657d 76 dblDutyCycle = (double) intDeltaT/intT;
dofydoink 0:20018747657d 77 mutDutyCycle.unlock();
dofydoink 0:20018747657d 78 }
dofydoink 0:20018747657d 79 }
dofydoink 0:20018747657d 80
dofydoink 0:20018747657d 81 Semaphore semPosCtrl(1);
dofydoink 0:20018747657d 82 Semaphore semDebug(1);
dofydoink 0:20018747657d 83 Semaphore semGate(1);
dofydoink 0:20018747657d 84
dofydoink 0:20018747657d 85 Timer timer;
dofydoink 0:20018747657d 86
dofydoink 0:20018747657d 87 long pulsesTest;
dofydoink 0:20018747657d 88
dofydoink 0:20018747657d 89 //define all pins
dofydoink 3:9bd35e5b05ba 90 SPI spi(PB_15, PB_14, PB_13); // mosi, miso, sclk //DO NOT USE D4 OR D5 !!!
dofydoink 3:9bd35e5b05ba 91
dofydoink 3:9bd35e5b05ba 92 DigitalOut cs_ADC(PB_12);
dofydoink 3:9bd35e5b05ba 93
dofydoink 3:9bd35e5b05ba 94 DigitalOut pinGate(PA_8);
dofydoink 3:9bd35e5b05ba 95
dofydoink 3:9bd35e5b05ba 96 AnalogIn pinDemand1(PA_0);
dofydoink 3:9bd35e5b05ba 97 AnalogIn pinDemand2(PA_1);
dofydoink 3:9bd35e5b05ba 98
dofydoink 3:9bd35e5b05ba 99 AnalogIn pinCurSense(PB_0);
dofydoink 0:20018747657d 100
dofydoink 3:9bd35e5b05ba 101 FastPWM pinPwmOutput(PC_8);
dofydoink 3:9bd35e5b05ba 102
dofydoink 3:9bd35e5b05ba 103 FastPWM pinSigOut(D2);
dofydoink 3:9bd35e5b05ba 104
dofydoink 3:9bd35e5b05ba 105 DigitalOut pinDirectionFwd(PA_13);//INB
dofydoink 3:9bd35e5b05ba 106 DigitalOut pinDirectionRev(PA_14);//INA
dofydoink 1:cb2859df7a4c 107
dofydoink 3:9bd35e5b05ba 108 //define all variables
dofydoink 3:9bd35e5b05ba 109
dofydoink 3:9bd35e5b05ba 110 int ii,jj,kk,nn, spiTick; //counting variables
dofydoink 3:9bd35e5b05ba 111
dofydoink 3:9bd35e5b05ba 112 volatile int slaveReceivePos;
dofydoink 3:9bd35e5b05ba 113 volatile int slaveReceiveVel;
dofydoink 0:20018747657d 114
dofydoink 3:9bd35e5b05ba 115 volatile int dataFlag;
dofydoink 3:9bd35e5b05ba 116 volatile int intFeedBack_pos;
dofydoink 3:9bd35e5b05ba 117 volatile int intFeedBack_pres;
dofydoink 3:9bd35e5b05ba 118
dofydoink 3:9bd35e5b05ba 119 int intChkSum_pos;
dofydoink 3:9bd35e5b05ba 120 int intChkSum_vel;
dofydoink 3:9bd35e5b05ba 121
dofydoink 3:9bd35e5b05ba 122 int intPar_pos;
dofydoink 3:9bd35e5b05ba 123 int intPar_vel;
dofydoink 3:9bd35e5b05ba 124
dofydoink 3:9bd35e5b05ba 125 //raw Data Readings
dofydoink 3:9bd35e5b05ba 126 int intPotRead = 0;
dofydoink 3:9bd35e5b05ba 127 int intPressureRead = 0;
dofydoink 3:9bd35e5b05ba 128 double analogReadingDemPos;
dofydoink 3:9bd35e5b05ba 129 double analogReadingDemVel;
dofydoink 3:9bd35e5b05ba 130 double analogReadingCurSens;
dofydoink 3:9bd35e5b05ba 131
dofydoink 3:9bd35e5b05ba 132 char buf[10];
dofydoink 3:9bd35e5b05ba 133 int dataReceived;
dofydoink 3:9bd35e5b05ba 134
dofydoink 3:9bd35e5b05ba 135
dofydoink 3:9bd35e5b05ba 136
dofydoink 3:9bd35e5b05ba 137
dofydoink 3:9bd35e5b05ba 138 //system state variables
dofydoink 3:9bd35e5b05ba 139 double dblPos[10];//current position in mm
dofydoink 3:9bd35e5b05ba 140 double dblPosFil[10];//current position in mm
dofydoink 3:9bd35e5b05ba 141 double dblLastPos;//previous position in mm
dofydoink 3:9bd35e5b05ba 142 double dblVel[10];//velocity in mm/s
dofydoink 3:9bd35e5b05ba 143 double dblAccel; //acceleration in mm/s^2
dofydoink 3:9bd35e5b05ba 144 double dblIntegral = 0;//integral of position;
dofydoink 3:9bd35e5b05ba 145
dofydoink 3:9bd35e5b05ba 146 double dblPotPositionRead;
dofydoink 3:9bd35e5b05ba 147
dofydoink 3:9bd35e5b05ba 148 double dblPosBias = 48.0;
dofydoink 3:9bd35e5b05ba 149
dofydoink 3:9bd35e5b05ba 150 double dblStartingPos;
dofydoink 3:9bd35e5b05ba 151
dofydoink 3:9bd35e5b05ba 152 //demand variables
dofydoink 3:9bd35e5b05ba 153 double dblPosD[10];
dofydoink 3:9bd35e5b05ba 154 double dblLastPosD;
dofydoink 3:9bd35e5b05ba 155 double dblVelD[10];
dofydoink 3:9bd35e5b05ba 156 double dblAccelD;
dofydoink 3:9bd35e5b05ba 157
dofydoink 3:9bd35e5b05ba 158 double dblTargetPos;
dofydoink 3:9bd35e5b05ba 159 double dblTargetVel;
dofydoink 3:9bd35e5b05ba 160 double dblLinearPath;
dofydoink 3:9bd35e5b05ba 161 double dblLastLinearPath;
dofydoink 3:9bd35e5b05ba 162 double dblPathSign;
dofydoink 3:9bd35e5b05ba 163
dofydoink 3:9bd35e5b05ba 164
dofydoink 3:9bd35e5b05ba 165
dofydoink 3:9bd35e5b05ba 166 double dblError;
dofydoink 3:9bd35e5b05ba 167 double dblLastError;
dofydoink 3:9bd35e5b05ba 168 double dblErrorDot;
dofydoink 3:9bd35e5b05ba 169
dofydoink 3:9bd35e5b05ba 170 //filter Variables
dofydoink 3:9bd35e5b05ba 171 int intPosFilOrder = 1;
dofydoink 3:9bd35e5b05ba 172 int intVelFilOrder = 1;
dofydoink 3:9bd35e5b05ba 173 int intDemPosFilOrder = 6;
dofydoink 3:9bd35e5b05ba 174 int intDemVelFilOrder = 6;
dofydoink 3:9bd35e5b05ba 175 int intOutFilOrder = 0;
dofydoink 3:9bd35e5b05ba 176
dofydoink 3:9bd35e5b05ba 177 double dblPressureVal_bar;
dofydoink 3:9bd35e5b05ba 178
dofydoink 3:9bd35e5b05ba 179 //controller variables
dofydoink 3:9bd35e5b05ba 180 double omega;
dofydoink 3:9bd35e5b05ba 181 double zeta;
dofydoink 3:9bd35e5b05ba 182
dofydoink 3:9bd35e5b05ba 183 double Kp = 20.0;
dofydoink 3:9bd35e5b05ba 184 double Ki = 2.0;
dofydoink 3:9bd35e5b05ba 185 double Kd = 1.5;
dofydoink 3:9bd35e5b05ba 186 double dblIntTerm;
dofydoink 3:9bd35e5b05ba 187 double dblIntLimit = 0.8;
dofydoink 3:9bd35e5b05ba 188 int RXFlag;
dofydoink 3:9bd35e5b05ba 189 double dblControlBias = 0.0;
dofydoink 3:9bd35e5b05ba 190
dofydoink 3:9bd35e5b05ba 191 double dblMotorVoltage = 12.0;
dofydoink 0:20018747657d 192
dofydoink 3:9bd35e5b05ba 193 double output[10];//controller output
dofydoink 3:9bd35e5b05ba 194
dofydoink 3:9bd35e5b05ba 195 //current limit variables
dofydoink 3:9bd35e5b05ba 196 double CurrentLimitSet;
dofydoink 3:9bd35e5b05ba 197 double dblCurrentLimitAmps = 3.0;
dofydoink 3:9bd35e5b05ba 198 double currentBuck;
dofydoink 3:9bd35e5b05ba 199 double currentBuckGain = 3.0;
dofydoink 3:9bd35e5b05ba 200
dofydoink 3:9bd35e5b05ba 201 Timer gateTimer;
dofydoink 3:9bd35e5b05ba 202
dofydoink 3:9bd35e5b05ba 203 //define custom Functions
dofydoink 3:9bd35e5b05ba 204
dofydoink 3:9bd35e5b05ba 205 bool CheckMessage(int msg) {
dofydoink 3:9bd35e5b05ba 206 // Find message parity
dofydoink 3:9bd35e5b05ba 207 short int count = 0;
dofydoink 3:9bd35e5b05ba 208 for(short int i=0; i<32; i++) {
dofydoink 3:9bd35e5b05ba 209 if( msg>>1 & (1<<i) ) count++;
dofydoink 3:9bd35e5b05ba 210 }
dofydoink 3:9bd35e5b05ba 211 int intParity = !(count%2);
dofydoink 3:9bd35e5b05ba 212 // Find message CheckSum
dofydoink 3:9bd35e5b05ba 213 int intChkSum = 0;
dofydoink 3:9bd35e5b05ba 214 int intTempVar = msg>>7;
dofydoink 3:9bd35e5b05ba 215 while(intTempVar > 0) {
dofydoink 3:9bd35e5b05ba 216 intChkSum += intTempVar%10;
dofydoink 3:9bd35e5b05ba 217 intTempVar = int(intTempVar/10);
dofydoink 3:9bd35e5b05ba 218 }
dofydoink 3:9bd35e5b05ba 219 // Check if parity, CheckSum and mesage type match
dofydoink 3:9bd35e5b05ba 220 bool isParityCorrect = (intParity == (msg&0x1));
dofydoink 3:9bd35e5b05ba 221 bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F));
dofydoink 3:9bd35e5b05ba 222 bool isCheckPassed = (isParityCorrect && isChkSumCorrect);
dofydoink 3:9bd35e5b05ba 223 return isCheckPassed;
dofydoink 3:9bd35e5b05ba 224 }
dofydoink 3:9bd35e5b05ba 225
dofydoink 3:9bd35e5b05ba 226 bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {
dofydoink 0:20018747657d 227
dofydoink 3:9bd35e5b05ba 228 unsigned int dummyMsg = 0x5555;
dofydoink 3:9bd35e5b05ba 229 bool isSuccess = true;
dofydoink 3:9bd35e5b05ba 230 unsigned int inboundMsg, typeBit;
dofydoink 3:9bd35e5b05ba 231 short int numPacketsReceived = 0;
dofydoink 3:9bd35e5b05ba 232 for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages
dofydoink 3:9bd35e5b05ba 233 while( gateTimer.read_us() < 500 ) {
dofydoink 3:9bd35e5b05ba 234 if( spiSlave->receive() ) {
dofydoink 3:9bd35e5b05ba 235 numPacketsReceived++;
dofydoink 3:9bd35e5b05ba 236
dofydoink 3:9bd35e5b05ba 237 inboundMsg = spiSlave->read();
dofydoink 3:9bd35e5b05ba 238 Mntr = 1;
dofydoink 3:9bd35e5b05ba 239 if(i==0) {
dofydoink 3:9bd35e5b05ba 240 spiSlave->reply(outboundMsgs[0]);
dofydoink 3:9bd35e5b05ba 241 } else if(i==1) {
dofydoink 3:9bd35e5b05ba 242 spiSlave->reply(outboundMsgs[1]);
dofydoink 3:9bd35e5b05ba 243 } else {
dofydoink 3:9bd35e5b05ba 244 spiSlave->reply(dummyMsg);
dofydoink 3:9bd35e5b05ba 245 }
dofydoink 3:9bd35e5b05ba 246 Mntr = 0;
dofydoink 3:9bd35e5b05ba 247
dofydoink 3:9bd35e5b05ba 248 if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply
dofydoink 3:9bd35e5b05ba 249 typeBit = inboundMsg>>1 & 0x1;
dofydoink 3:9bd35e5b05ba 250 inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;
dofydoink 3:9bd35e5b05ba 251 if( !CheckMessage(inboundMsg) ) {
dofydoink 3:9bd35e5b05ba 252 isSuccess = false;
dofydoink 3:9bd35e5b05ba 253 }
dofydoink 3:9bd35e5b05ba 254 }
dofydoink 3:9bd35e5b05ba 255 break;
dofydoink 3:9bd35e5b05ba 256 }
dofydoink 3:9bd35e5b05ba 257 }
dofydoink 3:9bd35e5b05ba 258 }
dofydoink 3:9bd35e5b05ba 259 if( numPacketsReceived != 3 ) {
dofydoink 3:9bd35e5b05ba 260 isSuccess = false;
dofydoink 3:9bd35e5b05ba 261 }
dofydoink 3:9bd35e5b05ba 262 return isSuccess;
dofydoink 3:9bd35e5b05ba 263 }
dofydoink 3:9bd35e5b05ba 264
dofydoink 3:9bd35e5b05ba 265 int Read14BitADC(int channel, DigitalOut CSpin)
dofydoink 3:9bd35e5b05ba 266 {
dofydoink 3:9bd35e5b05ba 267 char message;
dofydoink 3:9bd35e5b05ba 268 unsigned int outputA;
dofydoink 3:9bd35e5b05ba 269 unsigned int outputB;
dofydoink 3:9bd35e5b05ba 270 int output;
dofydoink 3:9bd35e5b05ba 271
dofydoink 3:9bd35e5b05ba 272 switch(channel)
dofydoink 0:20018747657d 273 {
dofydoink 3:9bd35e5b05ba 274 case 1:
dofydoink 0:20018747657d 275 message = CHAN_1;
dofydoink 3:9bd35e5b05ba 276 break;
dofydoink 3:9bd35e5b05ba 277
dofydoink 3:9bd35e5b05ba 278 case 2:
dofydoink 3:9bd35e5b05ba 279 message = CHAN_2;
dofydoink 3:9bd35e5b05ba 280 break;
dofydoink 3:9bd35e5b05ba 281
dofydoink 3:9bd35e5b05ba 282 case 3:
dofydoink 3:9bd35e5b05ba 283 message = CHAN_3;
dofydoink 3:9bd35e5b05ba 284 break;
dofydoink 3:9bd35e5b05ba 285 case 4:
dofydoink 3:9bd35e5b05ba 286 message = CHAN_4;
dofydoink 3:9bd35e5b05ba 287 break;
dofydoink 3:9bd35e5b05ba 288
dofydoink 3:9bd35e5b05ba 289 default:
dofydoink 3:9bd35e5b05ba 290 message = CHAN_1;
dofydoink 0:20018747657d 291 }
dofydoink 0:20018747657d 292
dofydoink 3:9bd35e5b05ba 293 spi.format(8,0);
dofydoink 3:9bd35e5b05ba 294 spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
dofydoink 3:9bd35e5b05ba 295
dofydoink 3:9bd35e5b05ba 296 CSpin.write(0);
dofydoink 3:9bd35e5b05ba 297 spi.write(message);
dofydoink 3:9bd35e5b05ba 298 spi.write(0x00);
dofydoink 3:9bd35e5b05ba 299 outputA = spi.write(0x00);
dofydoink 3:9bd35e5b05ba 300 outputB = spi.write(0x00);
dofydoink 3:9bd35e5b05ba 301 CSpin.write(1);
dofydoink 3:9bd35e5b05ba 302
dofydoink 3:9bd35e5b05ba 303 //convert result to sensible value
dofydoink 3:9bd35e5b05ba 304 outputA = outputA<<8;
dofydoink 3:9bd35e5b05ba 305 output = outputA | outputB;
dofydoink 3:9bd35e5b05ba 306 output = output>>2;
dofydoink 3:9bd35e5b05ba 307
dofydoink 3:9bd35e5b05ba 308 return output;
dofydoink 3:9bd35e5b05ba 309 }
dofydoink 3:9bd35e5b05ba 310
dofydoink 3:9bd35e5b05ba 311 void ADC_Config()
dofydoink 3:9bd35e5b05ba 312 {
dofydoink 3:9bd35e5b05ba 313
dofydoink 3:9bd35e5b05ba 314 unsigned int msg;
dofydoink 3:9bd35e5b05ba 315 spi.format(8,0);
dofydoink 3:9bd35e5b05ba 316 spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
dofydoink 3:9bd35e5b05ba 317
dofydoink 3:9bd35e5b05ba 318 msg = CHAN_1 | RANGE_CONFIG; //config channel 1 set Vref as
dofydoink 3:9bd35e5b05ba 319 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 320 spi.write(msg);
dofydoink 3:9bd35e5b05ba 321 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 322
dofydoink 3:9bd35e5b05ba 323 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 324 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 325 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 326
dofydoink 3:9bd35e5b05ba 327 msg = CHAN_2 | RANGE_CONFIG; //config channel 2
dofydoink 3:9bd35e5b05ba 328 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 329 spi.write(msg);
dofydoink 3:9bd35e5b05ba 330 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 331
dofydoink 3:9bd35e5b05ba 332 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 333 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 334 cs_ADC = 1;
dofydoink 0:20018747657d 335
dofydoink 3:9bd35e5b05ba 336 msg = CHAN_3 | RANGE_CONFIG; //config channel 3
dofydoink 3:9bd35e5b05ba 337 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 338 spi.write(msg);
dofydoink 3:9bd35e5b05ba 339 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 340
dofydoink 3:9bd35e5b05ba 341 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 342 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 343 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 344
dofydoink 3:9bd35e5b05ba 345 msg = CHAN_4 | RANGE_CONFIG; //config channel 4
dofydoink 3:9bd35e5b05ba 346 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 347 spi.write(msg);
dofydoink 3:9bd35e5b05ba 348 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 349
dofydoink 3:9bd35e5b05ba 350 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 351 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 352 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 353
dofydoink 3:9bd35e5b05ba 354 }
dofydoink 0:20018747657d 355
dofydoink 0:20018747657d 356
dofydoink 0:20018747657d 357 int SumDigits(int var)
dofydoink 0:20018747657d 358 {
dofydoink 0:20018747657d 359 int intSumResult = 0;
dofydoink 0:20018747657d 360 int intTempVar = var;
dofydoink 0:20018747657d 361 while (intTempVar >0)
dofydoink 0:20018747657d 362 {
dofydoink 0:20018747657d 363 intSumResult += intTempVar%10;
dofydoink 0:20018747657d 364 intTempVar = int(intTempVar/10);
dofydoink 0:20018747657d 365 //intSumResult += int(var/100)%100;
dofydoink 0:20018747657d 366 }
dofydoink 0:20018747657d 367
dofydoink 0:20018747657d 368 return intSumResult;
dofydoink 0:20018747657d 369 }
dofydoink 0:20018747657d 370
dofydoink 3:9bd35e5b05ba 371 //int EvenParityBitGen(int var)
dofydoink 3:9bd35e5b05ba 372 int OddParityBitGen(int var)
dofydoink 0:20018747657d 373 {
dofydoink 0:20018747657d 374 unsigned int count = 0, i, b = 1;
dofydoink 0:20018747657d 375
dofydoink 0:20018747657d 376 for(i = 0; i < 32; i++){
dofydoink 0:20018747657d 377 if( var & (b << i) ){count++;}
dofydoink 0:20018747657d 378 }
dofydoink 0:20018747657d 379
dofydoink 3:9bd35e5b05ba 380 if( (count % 2) ){
dofydoink 3:9bd35e5b05ba 381 return 0;
dofydoink 3:9bd35e5b05ba 382 }
dofydoink 0:20018747657d 383
dofydoink 0:20018747657d 384 return 1;
dofydoink 0:20018747657d 385 }
dofydoink 0:20018747657d 386
dofydoink 0:20018747657d 387 void CloseGate()
dofydoink 0:20018747657d 388 {
dofydoink 0:20018747657d 389 pinGate = 0;
dofydoink 0:20018747657d 390
dofydoink 0:20018747657d 391 }
dofydoink 0:20018747657d 392
dofydoink 3:9bd35e5b05ba 393
dofydoink 0:20018747657d 394
dofydoink 0:20018747657d 395
dofydoink 3:9bd35e5b05ba 396 void PositionControlPID()
dofydoink 3:9bd35e5b05ba 397 {
dofydoink 3:9bd35e5b05ba 398 while(1)
dofydoink 0:20018747657d 399 {
dofydoink 3:9bd35e5b05ba 400 semPosCtrl.wait();
dofydoink 3:9bd35e5b05ba 401
dofydoink 3:9bd35e5b05ba 402 Mntr2 = !Mntr2;
dofydoink 3:9bd35e5b05ba 403 //Mntr2 = 1 - Mntr2;//!led;
dofydoink 3:9bd35e5b05ba 404 pulsesTest = wheel.getPulses();
dofydoink 3:9bd35e5b05ba 405
dofydoink 3:9bd35e5b05ba 406 double testy = (double) abs(pulsesTest)/2000;
dofydoink 3:9bd35e5b05ba 407 pinSigOut.write(testy);
dofydoink 3:9bd35e5b05ba 408
dofydoink 3:9bd35e5b05ba 409 //take all readings
dofydoink 3:9bd35e5b05ba 410
dofydoink 3:9bd35e5b05ba 411 //sensor readings
dofydoink 3:9bd35e5b05ba 412
dofydoink 3:9bd35e5b05ba 413 intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
dofydoink 3:9bd35e5b05ba 414 intPressureRead = intPressureRead-1334;
dofydoink 3:9bd35e5b05ba 415
dofydoink 3:9bd35e5b05ba 416 dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0;
dofydoink 3:9bd35e5b05ba 417
dofydoink 3:9bd35e5b05ba 418
dofydoink 3:9bd35e5b05ba 419 intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);
dofydoink 3:9bd35e5b05ba 420 //printf("%d\r\n",intFeedBack_pres);
dofydoink 3:9bd35e5b05ba 421
dofydoink 3:9bd35e5b05ba 422
dofydoink 3:9bd35e5b05ba 423 //intFeedBack_pres = intFeedBack_pres>>5;
dofydoink 3:9bd35e5b05ba 424
dofydoink 3:9bd35e5b05ba 425 intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);//add checksum
dofydoink 3:9bd35e5b05ba 426 intFeedBack_pres = (intFeedBack_pres<<1);
dofydoink 3:9bd35e5b05ba 427 intFeedBack_pres = intFeedBack_pres | 0x0001; //add type (1 for pressure)
dofydoink 3:9bd35e5b05ba 428 intFeedBack_pres = (intFeedBack_pres <<1) | OddParityBitGen(intFeedBack_pres);//add parity
dofydoink 3:9bd35e5b05ba 429 intFeedBack_pres = intFeedBack_pres & 0xFFFF;
dofydoink 0:20018747657d 430
dofydoink 3:9bd35e5b05ba 431 unsigned short garb = 0x01;
dofydoink 3:9bd35e5b05ba 432
dofydoink 3:9bd35e5b05ba 433 intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 3:9bd35e5b05ba 434 dblPotPositionRead = (double) POT_2_MM*(intPotRead - POT_OFFSET);
dofydoink 3:9bd35e5b05ba 435
dofydoink 3:9bd35e5b05ba 436
dofydoink 3:9bd35e5b05ba 437 //demand Readings
dofydoink 2:aee7d4724915 438
dofydoink 3:9bd35e5b05ba 439
dofydoink 3:9bd35e5b05ba 440 //current reading
dofydoink 3:9bd35e5b05ba 441 analogReadingCurSens = (double) 0.3*pinCurSense.read()+0.7*analogReadingCurSens;
dofydoink 3:9bd35e5b05ba 442
dofydoink 3:9bd35e5b05ba 443 //convert units and filter
dofydoink 3:9bd35e5b05ba 444
dofydoink 3:9bd35e5b05ba 445 //get position and filter
dofydoink 3:9bd35e5b05ba 446 dblPos[0] = (double) pulsesTest*-0.0078125 + dblStartingPos;
dofydoink 3:9bd35e5b05ba 447 dblSensorDriftError = dblPotPositionRead - dblPos[0];
dofydoink 3:9bd35e5b05ba 448
dofydoink 3:9bd35e5b05ba 449 if(dblSensorDriftError > 2.0)//if encoder reading is seriously wrong
dofydoink 3:9bd35e5b05ba 450 {
dofydoink 3:9bd35e5b05ba 451 //dblPos[0] = dblPotPositionRead;
dofydoink 3:9bd35e5b05ba 452 }
dofydoink 3:9bd35e5b05ba 453
dofydoink 3:9bd35e5b05ba 454 //printf("%d, %f, %f\r\n",pulsesTest,dblPos[0],dblPotPositionRead);
dofydoink 3:9bd35e5b05ba 455 if(intPosFilOrder > 0)
dofydoink 3:9bd35e5b05ba 456 {
dofydoink 3:9bd35e5b05ba 457 for (ii = 1; ii<intPosFilOrder+1; ii++)
dofydoink 3:9bd35e5b05ba 458 {
dofydoink 3:9bd35e5b05ba 459 dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii];
dofydoink 3:9bd35e5b05ba 460 }
dofydoink 3:9bd35e5b05ba 461 }
dofydoink 3:9bd35e5b05ba 462 else
dofydoink 3:9bd35e5b05ba 463 {
dofydoink 3:9bd35e5b05ba 464 dblPos[intPosFilOrder] = dblPos[0];
dofydoink 3:9bd35e5b05ba 465 }
dofydoink 0:20018747657d 466
dofydoink 3:9bd35e5b05ba 467 //get velocity and filter
dofydoink 3:9bd35e5b05ba 468 dblVel[0] = dblPos[intPosFilOrder] - dblLastPos;
dofydoink 3:9bd35e5b05ba 469 if(intVelFilOrder>0)
dofydoink 3:9bd35e5b05ba 470 {
dofydoink 3:9bd35e5b05ba 471 for (ii = 1; ii<intVelFilOrder+1; ii++)
dofydoink 0:20018747657d 472 {
dofydoink 3:9bd35e5b05ba 473 dblVel[ii] = (double) 0.7*dblVel[ii-1] + 0.3*dblVel[ii];
dofydoink 0:20018747657d 474 }
dofydoink 3:9bd35e5b05ba 475 }
dofydoink 3:9bd35e5b05ba 476 else
dofydoink 3:9bd35e5b05ba 477 {
dofydoink 3:9bd35e5b05ba 478 dblVel[intVelFilOrder] = dblVel[0];
dofydoink 3:9bd35e5b05ba 479 }
dofydoink 3:9bd35e5b05ba 480
dofydoink 3:9bd35e5b05ba 481
dofydoink 3:9bd35e5b05ba 482 //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
dofydoink 3:9bd35e5b05ba 483
dofydoink 3:9bd35e5b05ba 484 intFeedBack_pos = (int) dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511;
dofydoink 3:9bd35e5b05ba 485
dofydoink 3:9bd35e5b05ba 486 if(intFeedBack_pos>511)
dofydoink 3:9bd35e5b05ba 487 {
dofydoink 3:9bd35e5b05ba 488 intFeedBack_pos = 511;
dofydoink 3:9bd35e5b05ba 489 }
dofydoink 3:9bd35e5b05ba 490
dofydoink 3:9bd35e5b05ba 491 if(intFeedBack_pos<0)
dofydoink 3:9bd35e5b05ba 492 {
dofydoink 3:9bd35e5b05ba 493 intFeedBack_pos = 0;
dofydoink 3:9bd35e5b05ba 494 }
dofydoink 3:9bd35e5b05ba 495
dofydoink 3:9bd35e5b05ba 496
dofydoink 3:9bd35e5b05ba 497 intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);//add checkSum
dofydoink 3:9bd35e5b05ba 498 intFeedBack_pos = intFeedBack_pos <<1; // add type (0 for position)
dofydoink 3:9bd35e5b05ba 499 intFeedBack_pos = (intFeedBack_pos <<1) | OddParityBitGen(intFeedBack_pos);//add parity
dofydoink 3:9bd35e5b05ba 500
dofydoink 3:9bd35e5b05ba 501 //intFeedBack = dblSensorDriftError*8191;
dofydoink 3:9bd35e5b05ba 502
dofydoink 3:9bd35e5b05ba 503
dofydoink 3:9bd35e5b05ba 504 ///////////////PATH GENERATION////////////////////////
dofydoink 3:9bd35e5b05ba 505 //work out next path point
dofydoink 3:9bd35e5b05ba 506 double dblPathDifference;
dofydoink 3:9bd35e5b05ba 507 dblPathDifference = dblTargetPos - dblLinearPath;
dofydoink 3:9bd35e5b05ba 508 dblPathSign = dblPathDifference/fabs(dblPathDifference); //is velocity positive or negative?
dofydoink 0:20018747657d 509
dofydoink 3:9bd35e5b05ba 510 //check if target has not been reached (with additional 1% of step to be sure)
dofydoink 3:9bd35e5b05ba 511 if (fabs(dblPathDifference) > 1.01*dblTargetVel*dblSampleTime_s) {
dofydoink 3:9bd35e5b05ba 512 dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
dofydoink 3:9bd35e5b05ba 513 }
dofydoink 3:9bd35e5b05ba 514 else { //if path is very close to target position
dofydoink 3:9bd35e5b05ba 515 dblLinearPath = dblTargetPos;
dofydoink 3:9bd35e5b05ba 516 }
dofydoink 3:9bd35e5b05ba 517
dofydoink 3:9bd35e5b05ba 518 //limit position
dofydoink 3:9bd35e5b05ba 519 if(dblLinearPath > MAX_POSITION_MM){
dofydoink 3:9bd35e5b05ba 520 dblLinearPath = MAX_POSITION_MM;
dofydoink 3:9bd35e5b05ba 521 }
dofydoink 3:9bd35e5b05ba 522 if (dblLinearPath < 0.0){
dofydoink 3:9bd35e5b05ba 523 dblLinearPath = 0.0;
dofydoink 3:9bd35e5b05ba 524 }
dofydoink 3:9bd35e5b05ba 525
dofydoink 3:9bd35e5b05ba 526 dblPosD[intDemPosFilOrder] = 0.2*dblLinearPath + 0.8*dblPosD[intDemPosFilOrder];
dofydoink 3:9bd35e5b05ba 527
dofydoink 3:9bd35e5b05ba 528 //make sure path is safe
dofydoink 3:9bd35e5b05ba 529 if (dblPosD[intDemPosFilOrder] > MAX_POSITION_MM) {
dofydoink 3:9bd35e5b05ba 530 dblPosD[intDemPosFilOrder] = MAX_POSITION_MM;
dofydoink 3:9bd35e5b05ba 531 }
dofydoink 3:9bd35e5b05ba 532 if (dblPosD[intDemPosFilOrder] < 0.0) {
dofydoink 3:9bd35e5b05ba 533 dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 3:9bd35e5b05ba 534 }
dofydoink 3:9bd35e5b05ba 535
dofydoink 3:9bd35e5b05ba 536 dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD;
dofydoink 3:9bd35e5b05ba 537
dofydoink 3:9bd35e5b05ba 538 ///////////////////////////////////////////////////// End of Path Generation
dofydoink 3:9bd35e5b05ba 539
dofydoink 3:9bd35e5b05ba 540
dofydoink 3:9bd35e5b05ba 541 //run PID calculations
dofydoink 3:9bd35e5b05ba 542 //get errors
dofydoink 3:9bd35e5b05ba 543 dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
dofydoink 3:9bd35e5b05ba 544 dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
dofydoink 3:9bd35e5b05ba 545 //get integral
dofydoink 3:9bd35e5b05ba 546 //printf("%f\r\n",dblError);
dofydoink 3:9bd35e5b05ba 547
dofydoink 3:9bd35e5b05ba 548
dofydoink 3:9bd35e5b05ba 549 dblIntTerm = dblIntTerm + Ki*dblError;
dofydoink 3:9bd35e5b05ba 550
dofydoink 3:9bd35e5b05ba 551 //limit integral term
dofydoink 3:9bd35e5b05ba 552 if (dblIntTerm > dblIntLimit)
dofydoink 3:9bd35e5b05ba 553 {
dofydoink 3:9bd35e5b05ba 554 dblIntTerm = dblIntLimit;
dofydoink 3:9bd35e5b05ba 555 }
dofydoink 3:9bd35e5b05ba 556 if (dblIntTerm < -1.0*dblIntLimit)
dofydoink 3:9bd35e5b05ba 557 {
dofydoink 3:9bd35e5b05ba 558 dblIntTerm = (double) -1.0*dblIntLimit;
dofydoink 3:9bd35e5b05ba 559 }
dofydoink 3:9bd35e5b05ba 560
dofydoink 3:9bd35e5b05ba 561 if(fabs(dblError) <0.01)
dofydoink 3:9bd35e5b05ba 562 {
dofydoink 3:9bd35e5b05ba 563 dblError = 0.0;
dofydoink 3:9bd35e5b05ba 564 dblErrorDot = 0.0;
dofydoink 0:20018747657d 565
dofydoink 3:9bd35e5b05ba 566 }
dofydoink 0:20018747657d 567
dofydoink 3:9bd35e5b05ba 568 if (fabs(dblErrorDot) < 0.1)
dofydoink 3:9bd35e5b05ba 569 {
dofydoink 3:9bd35e5b05ba 570 dblErrorDot = 0.0;
dofydoink 3:9bd35e5b05ba 571 }
dofydoink 3:9bd35e5b05ba 572
dofydoink 3:9bd35e5b05ba 573 //calculate output
dofydoink 3:9bd35e5b05ba 574 output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
dofydoink 3:9bd35e5b05ba 575
dofydoink 3:9bd35e5b05ba 576 //limit output
dofydoink 3:9bd35e5b05ba 577 if (output[0] > 0.95)
dofydoink 3:9bd35e5b05ba 578 {
dofydoink 3:9bd35e5b05ba 579 output[0] = 0.95;
dofydoink 3:9bd35e5b05ba 580 }
dofydoink 3:9bd35e5b05ba 581 if (output[0] < -0.95)
dofydoink 3:9bd35e5b05ba 582 {
dofydoink 3:9bd35e5b05ba 583 output[0] = -0.95;
dofydoink 3:9bd35e5b05ba 584 }
dofydoink 3:9bd35e5b05ba 585
dofydoink 3:9bd35e5b05ba 586 if(intOutFilOrder>0)
dofydoink 3:9bd35e5b05ba 587 {
dofydoink 3:9bd35e5b05ba 588 for (ii = 1; ii < intOutFilOrder+1; ii++)
dofydoink 0:20018747657d 589 {
dofydoink 3:9bd35e5b05ba 590 output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
dofydoink 0:20018747657d 591 }
dofydoink 3:9bd35e5b05ba 592 }
dofydoink 3:9bd35e5b05ba 593 else
dofydoink 3:9bd35e5b05ba 594 {
dofydoink 3:9bd35e5b05ba 595 output[intOutFilOrder] = output[0];
dofydoink 3:9bd35e5b05ba 596 }
dofydoink 3:9bd35e5b05ba 597
dofydoink 3:9bd35e5b05ba 598 //limit current
dofydoink 3:9bd35e5b05ba 599 if (analogReadingCurSens> CurrentLimitSet)
dofydoink 3:9bd35e5b05ba 600 {
dofydoink 3:9bd35e5b05ba 601 currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
dofydoink 3:9bd35e5b05ba 602 }
dofydoink 3:9bd35e5b05ba 603 else
dofydoink 3:9bd35e5b05ba 604 {
dofydoink 3:9bd35e5b05ba 605 currentBuck = 1.0;
dofydoink 3:9bd35e5b05ba 606 }
dofydoink 3:9bd35e5b05ba 607
dofydoink 3:9bd35e5b05ba 608 output[intOutFilOrder] = currentBuck*output[intOutFilOrder];
dofydoink 3:9bd35e5b05ba 609 //end Current limit
dofydoink 3:9bd35e5b05ba 610
dofydoink 3:9bd35e5b05ba 611 //find direction
dofydoink 3:9bd35e5b05ba 612 if(output[intOutFilOrder] >=0.0)
dofydoink 3:9bd35e5b05ba 613 {
dofydoink 3:9bd35e5b05ba 614 pinDirectionFwd = 1;
dofydoink 3:9bd35e5b05ba 615 pinDirectionRev = 0;
dofydoink 3:9bd35e5b05ba 616 dblControlBias = 0.0;
dofydoink 3:9bd35e5b05ba 617 }
dofydoink 3:9bd35e5b05ba 618 else
dofydoink 3:9bd35e5b05ba 619 {
dofydoink 3:9bd35e5b05ba 620 pinDirectionFwd = 0;
dofydoink 3:9bd35e5b05ba 621 pinDirectionRev = 1;
dofydoink 3:9bd35e5b05ba 622 dblControlBias = 0.0;
dofydoink 3:9bd35e5b05ba 623 }
dofydoink 3:9bd35e5b05ba 624
dofydoink 3:9bd35e5b05ba 625 pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias);
dofydoink 3:9bd35e5b05ba 626
dofydoink 3:9bd35e5b05ba 627 //update all past variables
dofydoink 3:9bd35e5b05ba 628 dblLastPos = dblPos[intPosFilOrder];
dofydoink 3:9bd35e5b05ba 629 dblLastPosD = dblPosD[intDemPosFilOrder];
dofydoink 3:9bd35e5b05ba 630
dofydoink 3:9bd35e5b05ba 631 //GateControl();
dofydoink 3:9bd35e5b05ba 632 gateTimer.reset();
dofydoink 3:9bd35e5b05ba 633 pinGate = 1;
dofydoink 3:9bd35e5b05ba 634 unsigned int outboundMsgs[2] = { intFeedBack_pos , intFeedBack_pres };
dofydoink 3:9bd35e5b05ba 635 unsigned int inboundMsgsData[2] = {0,0};
dofydoink 3:9bd35e5b05ba 636 while(gateTimer.read_us() < 500) {
dofydoink 0:20018747657d 637
dofydoink 3:9bd35e5b05ba 638 if(slave.receive()) {
dofydoink 3:9bd35e5b05ba 639
dofydoink 3:9bd35e5b05ba 640 bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);
dofydoink 0:20018747657d 641
dofydoink 3:9bd35e5b05ba 642 if( isSPIsuccess ) {
dofydoink 3:9bd35e5b05ba 643 dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; // dblMaxPos should be a constant double MAX_POSITION_MM
dofydoink 3:9bd35e5b05ba 644 if(dblTargetPos>MAX_POSITION_MM) { // Limit demand to ensure safety
dofydoink 3:9bd35e5b05ba 645 dblTargetPos = MAX_POSITION_MM;
dofydoink 3:9bd35e5b05ba 646 } else if(dblTargetPos<0.0) {
dofydoink 3:9bd35e5b05ba 647 dblTargetPos = 0.0;
dofydoink 3:9bd35e5b05ba 648 }
dofydoink 3:9bd35e5b05ba 649
dofydoink 3:9bd35e5b05ba 650 dblTargetVel = (double)MAX_SPEED_MMPS*inboundMsgsData[1]/511;
dofydoink 3:9bd35e5b05ba 651
dofydoink 3:9bd35e5b05ba 652 if(dblTargetVel>MAX_SPEED_MMPS) {
dofydoink 3:9bd35e5b05ba 653 dblTargetVel = MAX_SPEED_MMPS;
dofydoink 3:9bd35e5b05ba 654 } else if(dblTargetVel<0.0) {
dofydoink 3:9bd35e5b05ba 655 dblTargetPos = 0.0;
dofydoink 3:9bd35e5b05ba 656 }
dofydoink 3:9bd35e5b05ba 657
dofydoink 3:9bd35e5b05ba 658 break;
dofydoink 2:aee7d4724915 659 }
dofydoink 0:20018747657d 660
dofydoink 0:20018747657d 661 }
dofydoink 0:20018747657d 662
dofydoink 0:20018747657d 663 }
dofydoink 3:9bd35e5b05ba 664
dofydoink 3:9bd35e5b05ba 665 pinGate = 0;
dofydoink 3:9bd35e5b05ba 666 //printf("EncPos: %f \t TargetPos: %f \t TargetVel: %f \t LinPath: %f \t DemPos: %f\r\n", dblPos[intPosFilOrder],dblTargetPos, dblTargetVel,dblLinearPath, dblPosD[intDemPosFilOrder]);
dofydoink 3:9bd35e5b05ba 667
dofydoink 3:9bd35e5b05ba 668 //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail);
dofydoink 3:9bd35e5b05ba 669
dofydoink 3:9bd35e5b05ba 670 }
dofydoink 3:9bd35e5b05ba 671 }
dofydoink 0:20018747657d 672
dofydoink 0:20018747657d 673
dofydoink 3:9bd35e5b05ba 674 //configure all control parameters
dofydoink 3:9bd35e5b05ba 675 void ControlParameterConfig()
dofydoink 3:9bd35e5b05ba 676 {
dofydoink 3:9bd35e5b05ba 677 Kp = Kp/dblMotorVoltage;
dofydoink 3:9bd35e5b05ba 678 Kd = Kd/dblSampleTime_s/dblMotorVoltage;
dofydoink 3:9bd35e5b05ba 679 Ki = Ki*dblSampleTime_s/dblMotorVoltage;
dofydoink 3:9bd35e5b05ba 680 }
dofydoink 0:20018747657d 681
dofydoink 0:20018747657d 682 Ticker debugTicker;
dofydoink 0:20018747657d 683
dofydoink 0:20018747657d 684 void startPositionControl()
dofydoink 0:20018747657d 685 {
dofydoink 0:20018747657d 686 semPosCtrl.release();
dofydoink 0:20018747657d 687 }
dofydoink 0:20018747657d 688
dofydoink 0:20018747657d 689 void startDebug()
dofydoink 0:20018747657d 690 {
dofydoink 0:20018747657d 691 semDebug.release();
dofydoink 0:20018747657d 692 }
dofydoink 0:20018747657d 693
dofydoink 0:20018747657d 694 Ticker positionCtrlTicker;
dofydoink 0:20018747657d 695
dofydoink 0:20018747657d 696 int main() {
dofydoink 2:aee7d4724915 697 pinGate = 0;
dofydoink 2:aee7d4724915 698
dofydoink 0:20018747657d 699 cs_ADC = 1;
dofydoink 2:aee7d4724915 700 Mntr = 0;
dofydoink 2:aee7d4724915 701 Mntr2 = 0;
dofydoink 0:20018747657d 702 pinPwmOutput.period_us(50);
dofydoink 0:20018747657d 703 printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n");
dofydoink 0:20018747657d 704 wait(0.5);
dofydoink 0:20018747657d 705
dofydoink 0:20018747657d 706 timer.start();
dofydoink 0:20018747657d 707 gateTimer.start();
dofydoink 0:20018747657d 708 //cs_ADC = 1;
dofydoink 0:20018747657d 709
dofydoink 0:20018747657d 710 ControlParameterConfig();
dofydoink 0:20018747657d 711 // for (ii = 0; ii< 10; ii++)
dofydoink 0:20018747657d 712 // {
dofydoink 0:20018747657d 713 // uintPotRead = Read14BitADC(3, cs_ADC);//read potentiometer
dofydoink 0:20018747657d 714 // dblStartingPos = (double) POT_2_MM*uintPotRead - dblPosBias;
dofydoink 0:20018747657d 715 // }
dofydoink 0:20018747657d 716 //
dofydoink 0:20018747657d 717 slave.format(16,2);
dofydoink 0:20018747657d 718 slave.frequency(10000000);
dofydoink 2:aee7d4724915 719
dofydoink 0:20018747657d 720 dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 0:20018747657d 721 slaveReceivePos = 0.0;
dofydoink 0:20018747657d 722 slaveReceiveVel = 0.0;
dofydoink 0:20018747657d 723 wait(0.1);
dofydoink 0:20018747657d 724 ADC_Config();
dofydoink 0:20018747657d 725 wait(0.1);
dofydoink 0:20018747657d 726 ADC_Config();
dofydoink 0:20018747657d 727 wait(0.1);
dofydoink 0:20018747657d 728 intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 0:20018747657d 729 //intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 0:20018747657d 730 dblStartingPos = (double) POT_2_MM*(intPotRead - POT_OFFSET);
dofydoink 2:aee7d4724915 731 dblLinearPath = dblStartingPos;
dofydoink 0:20018747657d 732 dblTargetPos = dblStartingPos;
dofydoink 3:9bd35e5b05ba 733 dblPos[intPosFilOrder] = dblStartingPos;
dofydoink 2:aee7d4724915 734 dblTargetVel = 0.0;
dofydoink 0:20018747657d 735 dblPosD[intDemPosFilOrder] = dblStartingPos;
dofydoink 0:20018747657d 736
dofydoink 2:aee7d4724915 737 printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos);
dofydoink 0:20018747657d 738
dofydoink 0:20018747657d 739 //calculate/convert variables
dofydoink 0:20018747657d 740
dofydoink 0:20018747657d 741 CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
dofydoink 2:aee7d4724915 742 slave.reply(0x5555);
dofydoink 0:20018747657d 743 PositionControlThread.start(PositionControlPID);
dofydoink 3:9bd35e5b05ba 744
dofydoink 0:20018747657d 745 positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
dofydoink 0:20018747657d 746
dofydoink 0:20018747657d 747 intFeedBack_pos = 0;
dofydoink 0:20018747657d 748 intFeedBack_pres = 0;
dofydoink 0:20018747657d 749
dofydoink 0:20018747657d 750
dofydoink 0:20018747657d 751 while(1)
dofydoink 0:20018747657d 752 {
dofydoink 0:20018747657d 753 Thread::wait(osWaitForever);
dofydoink 0:20018747657d 754 }
dofydoink 3:9bd35e5b05ba 755 }