MorphGI / Mbed OS MGI_Rebuild_MidLevel_9_0-Basic_PWM

Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.

Dependencies:   ros_lib_kinetic

main.cpp@42:5a5ad23a4bb1, 2021-06-24 (annotated)

Committer:
dofydoink
Date:
Thu Jun 24 20:34:47 2021 +0000
Revision:
42:5a5ad23a4bb1
Parent:
36:4459be8296e9 
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // STANDARD IMPORTS
WD40andTape 7:5b6a2cefbf3b 2 #include "math.h"
WD40andTape 7:5b6a2cefbf3b 3 // MBED IMPORTS
dofydoink 0:607bc887b6e0 4 #include "mbed.h"
dofydoink 42:5a5ad23a4bb1 5
WD40andTape 7:5b6a2cefbf3b 6 // CUSTOM IMPORTS
dofydoink 12:595ed862e52f 7 #include "MLSettings.h"
dofydoink 42:5a5ad23a4bb1 8 //#include "HLComms.h"
dofydoink 42:5a5ad23a4bb1 9
dofydoink 42:5a5ad23a4bb1 10 #define N_CHANNELS 4
dofydoink 0:607bc887b6e0 11
WD40andTape 21:0b10d8e615d1 12 // DEMAND VARIABLES
dofydoink 11:7029367a1840 13
WD40andTape 7:5b6a2cefbf3b 14 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 42:5a5ad23a4bb1 15
dofydoink 42:5a5ad23a4bb1 16 //HLComms hlcomms(HL_COMMS_FREQ_HZ);
dofydoink 42:5a5ad23a4bb1 17
dofydoink 42:5a5ad23a4bb1 18 double limDouble(double var, double min, double max){
dofydoink 42:5a5ad23a4bb1 19 double output = var;
dofydoink 42:5a5ad23a4bb1 20 if(var <min){
dofydoink 42:5a5ad23a4bb1 21 output = min;
dofydoink 42:5a5ad23a4bb1 22 }
dofydoink 42:5a5ad23a4bb1 23 if (var>max){
dofydoink 42:5a5ad23a4bb1 24 output = max;
dofydoink 42:5a5ad23a4bb1 25 }
dofydoink 42:5a5ad23a4bb1 26 return output;
dofydoink 42:5a5ad23a4bb1 27 }
dofydoink 42:5a5ad23a4bb1 28
dofydoink 42:5a5ad23a4bb1 29
dofydoink 42:5a5ad23a4bb1 30 //Ticker setDemandsTicker;
dofydoink 42:5a5ad23a4bb1 31
dofydoink 42:5a5ad23a4bb1 32
dofydoink 42:5a5ad23a4bb1 33 //joystick input pins
dofydoink 42:5a5ad23a4bb1 34
dofydoink 42:5a5ad23a4bb1 35 InterruptIn pinZeroIn (PD_4);
dofydoink 42:5a5ad23a4bb1 36 InterruptIn pinCycleIn (PD_6);
dofydoink 42:5a5ad23a4bb1 37 InterruptIn pinSucIn (PD_7);
dofydoink 42:5a5ad23a4bb1 38 InterruptIn pinGasIn (PF_5);
dofydoink 42:5a5ad23a4bb1 39 InterruptIn pinDebugIn (PF_9);
dofydoink 42:5a5ad23a4bb1 40 InterruptIn pinFlushIn (PG_1);
dofydoink 42:5a5ad23a4bb1 41
dofydoink 42:5a5ad23a4bb1 42 DigitalIn pinWashIn (PD_5);
dofydoink 42:5a5ad23a4bb1 43 DigitalIn pinFwd (PD_3);
dofydoink 42:5a5ad23a4bb1 44 DigitalIn pinRev (PC_3);
dofydoink 42:5a5ad23a4bb1 45
dofydoink 42:5a5ad23a4bb1 46
dofydoink 42:5a5ad23a4bb1 47 AnalogIn pinJSX (PC_0);
dofydoink 42:5a5ad23a4bb1 48 AnalogIn pinJSY (PF_3);
dofydoink 42:5a5ad23a4bb1 49
dofydoink 42:5a5ad23a4bb1 50 DigitalOut pinFlushOut (PG_0);
dofydoink 42:5a5ad23a4bb1 51 DigitalOut pinGasOut (PD_0);
dofydoink 42:5a5ad23a4bb1 52 DigitalOut pinWashOut (PF_1);
dofydoink 42:5a5ad23a4bb1 53 DigitalOut pinSucOut (PD_1);
dofydoink 42:5a5ad23a4bb1 54 DigitalOut pinThirdPump(PF_2);
dofydoink 42:5a5ad23a4bb1 55 DigitalOut pinJetOut (PF_0);
dofydoink 42:5a5ad23a4bb1 56
dofydoink 42:5a5ad23a4bb1 57 //DigitalIn arPins[7] = {pinZeroIn, pinCycleIn, pinSucIn, pinWashIn, pinGasIn, pinFwd, pinRev};
dofydoink 42:5a5ad23a4bb1 58
dofydoink 42:5a5ad23a4bb1 59 Thread threadReadHHC(osPriorityHigh);
dofydoink 42:5a5ad23a4bb1 60 Thread threadSendFeedback(osPriorityNormal);
dofydoink 42:5a5ad23a4bb1 61
dofydoink 42:5a5ad23a4bb1 62 Semaphore semReadHHC(1);
dofydoink 42:5a5ad23a4bb1 63 Semaphore semSendFeedback(1);
dofydoink 42:5a5ad23a4bb1 64 Semaphore semReceiveAndReplan(1);
dofydoink 42:5a5ad23a4bb1 65
dofydoink 42:5a5ad23a4bb1 66 //Ticker SendSensorDataTicker;
dofydoink 42:5a5ad23a4bb1 67 Ticker tickerReadHHC;
dofydoink 42:5a5ad23a4bb1 68 Ticker tickerSendFeedback;
dofydoink 42:5a5ad23a4bb1 69
dofydoink 42:5a5ad23a4bb1 70 void signalReadHHC(){
dofydoink 42:5a5ad23a4bb1 71 semReadHHC.release();
dofydoink 42:5a5ad23a4bb1 72 }
dofydoink 42:5a5ad23a4bb1 73
dofydoink 42:5a5ad23a4bb1 74 void signalSendFeedback() {
dofydoink 42:5a5ad23a4bb1 75 semSendFeedback.release();
dofydoink 42:5a5ad23a4bb1 76 }
dofydoink 42:5a5ad23a4bb1 77
dofydoink 42:5a5ad23a4bb1 78 //support function states;
dofydoink 42:5a5ad23a4bb1 79
dofydoink 42:5a5ad23a4bb1 80 bool isZero = 0;
dofydoink 42:5a5ad23a4bb1 81 bool isCycle = 0;
dofydoink 42:5a5ad23a4bb1 82 bool isSuc = 0;
dofydoink 42:5a5ad23a4bb1 83 bool isWash = 0;
dofydoink 42:5a5ad23a4bb1 84 bool isGas = 0;
dofydoink 42:5a5ad23a4bb1 85 bool isFwd = 0;
dofydoink 42:5a5ad23a4bb1 86 bool isRev = 0;
dofydoink 42:5a5ad23a4bb1 87 bool isBusy = 0;
dofydoink 42:5a5ad23a4bb1 88 bool DemPos=0;
dofydoink 42:5a5ad23a4bb1 89 bool isDebug = 0;
dofydoink 42:5a5ad23a4bb1 90
dofydoink 42:5a5ad23a4bb1 91 //set up PWM output pins for Demand POSITION AND VELOCITY
dofydoink 42:5a5ad23a4bb1 92 PwmOut pinPosOut[N_CHANNELS] = {PB_15, PB_8, PB_9 ,PC_6};
dofydoink 42:5a5ad23a4bb1 93 PwmOut pinVelOut[N_CHANNELS] = {PB_10, PA_0, PB_11 ,PB_3};
dofydoink 42:5a5ad23a4bb1 94
WD40andTape 3:c83291bf9fd2 95
dofydoink 42:5a5ad23a4bb1 96 void FlushActivate(){
dofydoink 42:5a5ad23a4bb1 97 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 98 pinFlushOut = 1;
dofydoink 42:5a5ad23a4bb1 99 }
dofydoink 42:5a5ad23a4bb1 100 }
dofydoink 42:5a5ad23a4bb1 101
dofydoink 42:5a5ad23a4bb1 102 void FlushStop(){
dofydoink 42:5a5ad23a4bb1 103 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 104 pinFlushOut = 0;
dofydoink 42:5a5ad23a4bb1 105 }
dofydoink 42:5a5ad23a4bb1 106 }
dofydoink 42:5a5ad23a4bb1 107
dofydoink 42:5a5ad23a4bb1 108 void ZeroActivate(){
dofydoink 42:5a5ad23a4bb1 109 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 110 isZero = 1;
dofydoink 42:5a5ad23a4bb1 111 }
dofydoink 42:5a5ad23a4bb1 112 }
dofydoink 42:5a5ad23a4bb1 113 void ZeroStop(){
dofydoink 42:5a5ad23a4bb1 114
dofydoink 42:5a5ad23a4bb1 115 }
dofydoink 42:5a5ad23a4bb1 116
dofydoink 42:5a5ad23a4bb1 117 void DebugActivate(){
dofydoink 42:5a5ad23a4bb1 118 isDebug = 1;
dofydoink 42:5a5ad23a4bb1 119 }
dofydoink 42:5a5ad23a4bb1 120
dofydoink 42:5a5ad23a4bb1 121 void DebugStop(){
dofydoink 42:5a5ad23a4bb1 122 isDebug = 0;
dofydoink 42:5a5ad23a4bb1 123 }
dofydoink 42:5a5ad23a4bb1 124
dofydoink 42:5a5ad23a4bb1 125 void CycleActivate(){
dofydoink 42:5a5ad23a4bb1 126 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 127 isCycle = 1;
dofydoink 42:5a5ad23a4bb1 128 }
dofydoink 42:5a5ad23a4bb1 129 }
dofydoink 42:5a5ad23a4bb1 130 void CycleStop(){
dofydoink 42:5a5ad23a4bb1 131
dofydoink 42:5a5ad23a4bb1 132 }
dofydoink 42:5a5ad23a4bb1 133
dofydoink 42:5a5ad23a4bb1 134 void GasActivate(){
dofydoink 42:5a5ad23a4bb1 135 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 136 pinGasOut = 1;
dofydoink 42:5a5ad23a4bb1 137 }
dofydoink 42:5a5ad23a4bb1 138 }
dofydoink 42:5a5ad23a4bb1 139 void GasStop(){
dofydoink 42:5a5ad23a4bb1 140 pinGasOut = 0;
dofydoink 42:5a5ad23a4bb1 141 }
dofydoink 42:5a5ad23a4bb1 142
dofydoink 42:5a5ad23a4bb1 143 void SucActivate(){
dofydoink 42:5a5ad23a4bb1 144 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 145 pinSucOut = 1;
dofydoink 42:5a5ad23a4bb1 146 }
dofydoink 42:5a5ad23a4bb1 147 }
dofydoink 42:5a5ad23a4bb1 148 void SucStop(){
dofydoink 42:5a5ad23a4bb1 149 pinSucOut = 0;
dofydoink 42:5a5ad23a4bb1 150 }
dofydoink 42:5a5ad23a4bb1 151
dofydoink 42:5a5ad23a4bb1 152 bool isChange = 1;
dofydoink 42:5a5ad23a4bb1 153
dofydoink 42:5a5ad23a4bb1 154 Timer timer1;
dofydoink 42:5a5ad23a4bb1 155
dofydoink 42:5a5ad23a4bb1 156 Ticker DemTicker;
dofydoink 42:5a5ad23a4bb1 157
dofydoink 42:5a5ad23a4bb1 158 int intAnchorDirection = 0;
dofydoink 42:5a5ad23a4bb1 159
dofydoink 42:5a5ad23a4bb1 160
dofydoink 42:5a5ad23a4bb1 161
dofydoink 42:5a5ad23a4bb1 162 //global variables
dofydoink 42:5a5ad23a4bb1 163 double dblSensitivity = 1.0;
dofydoink 42:5a5ad23a4bb1 164 double dblDriveStep = 0.05;
dofydoink 42:5a5ad23a4bb1 165 bool isSteerLock = 0;
dofydoink 42:5a5ad23a4bb1 166 double dblAlpha;
dofydoink 42:5a5ad23a4bb1 167 double dblBeta;
dofydoink 42:5a5ad23a4bb1 168 double dblTheta;
dofydoink 42:5a5ad23a4bb1 169 double dblPhi;
dofydoink 42:5a5ad23a4bb1 170 double dblS;
dofydoink 42:5a5ad23a4bb1 171 double dblSdot;
dofydoink 42:5a5ad23a4bb1 172 double dblX[3];
dofydoink 42:5a5ad23a4bb1 173 double dblXdot[3];
dofydoink 42:5a5ad23a4bb1 174 double dblAlphaDot;
dofydoink 42:5a5ad23a4bb1 175 double dblBetaDot;
dofydoink 42:5a5ad23a4bb1 176 double dblDeltaAlpha;
dofydoink 42:5a5ad23a4bb1 177 double dblDeltaBeta;
dofydoink 42:5a5ad23a4bb1 178
dofydoink 42:5a5ad23a4bb1 179 double x_axis_sign = -1.0;
dofydoink 42:5a5ad23a4bb1 180
dofydoink 42:5a5ad23a4bb1 181 //kinematic constants
dofydoink 42:5a5ad23a4bb1 182 double K_theta = 0.77;// rad/mL
dofydoink 42:5a5ad23a4bb1 183 double d_theta = 0.3;// no unit
dofydoink 0:607bc887b6e0 184
dofydoink 42:5a5ad23a4bb1 185 void SendFeedback(){
dofydoink 42:5a5ad23a4bb1 186 //support function interrupt states
dofydoink 42:5a5ad23a4bb1 187 while(1){
dofydoink 42:5a5ad23a4bb1 188 semSendFeedback.wait();
dofydoink 42:5a5ad23a4bb1 189 if(!isBusy){
dofydoink 42:5a5ad23a4bb1 190 if(pinFwd.read()){
dofydoink 42:5a5ad23a4bb1 191 isFwd = 1;
dofydoink 42:5a5ad23a4bb1 192 } else {
dofydoink 42:5a5ad23a4bb1 193 isFwd = 0;
dofydoink 42:5a5ad23a4bb1 194 }
dofydoink 42:5a5ad23a4bb1 195
dofydoink 42:5a5ad23a4bb1 196 if(pinRev.read()){
dofydoink 42:5a5ad23a4bb1 197 isRev = 1;
dofydoink 42:5a5ad23a4bb1 198 } else {
dofydoink 42:5a5ad23a4bb1 199 isRev = 0;
dofydoink 42:5a5ad23a4bb1 200 }
dofydoink 42:5a5ad23a4bb1 201
dofydoink 42:5a5ad23a4bb1 202 if(pinSucIn.read()){
dofydoink 42:5a5ad23a4bb1 203 isSuc = 1;
dofydoink 42:5a5ad23a4bb1 204 } else {
dofydoink 42:5a5ad23a4bb1 205 isSuc = 0;
dofydoink 42:5a5ad23a4bb1 206 }
dofydoink 42:5a5ad23a4bb1 207
dofydoink 42:5a5ad23a4bb1 208 if(pinWashIn.read()){
dofydoink 42:5a5ad23a4bb1 209 isWash = 1;
dofydoink 42:5a5ad23a4bb1 210 } else {
dofydoink 42:5a5ad23a4bb1 211 isWash = 0;
dofydoink 42:5a5ad23a4bb1 212 }
dofydoink 42:5a5ad23a4bb1 213
dofydoink 42:5a5ad23a4bb1 214 if(pinGasIn.read()){
dofydoink 42:5a5ad23a4bb1 215 isGas = 1;
dofydoink 42:5a5ad23a4bb1 216 } else {
dofydoink 42:5a5ad23a4bb1 217 isGas = 0;
dofydoink 42:5a5ad23a4bb1 218 }
dofydoink 42:5a5ad23a4bb1 219
dofydoink 42:5a5ad23a4bb1 220
dofydoink 42:5a5ad23a4bb1 221 //
dofydoink 42:5a5ad23a4bb1 222 //printf("\r\n");
dofydoink 42:5a5ad23a4bb1 223 //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t", isZero, isGas, isSuc, isRev, isWash, isCycle, isFwd, isDebug);
dofydoink 42:5a5ad23a4bb1 224 //
dofydoink 42:5a5ad23a4bb1 225 // double dblJSXprint = pinJSX.read();
dofydoink 42:5a5ad23a4bb1 226 // double dblJSYprint = pinJSY.read();
dofydoink 42:5a5ad23a4bb1 227 // printf("X:%f\tY:%f\t", dblJSXprint, dblJSYprint);
dofydoink 42:5a5ad23a4bb1 228
dofydoink 42:5a5ad23a4bb1 229 // printf("%f\t",dblAlphaDot);
dofydoink 42:5a5ad23a4bb1 230 // printf("%f\t",dblBetaDot);
dofydoink 42:5a5ad23a4bb1 231 // printf(":: ");
dofydoink 42:5a5ad23a4bb1 232 //
dofydoink 42:5a5ad23a4bb1 233 // printf("%f\t",dblAlpha);
dofydoink 42:5a5ad23a4bb1 234 // printf("%f\t",dblBeta);
dofydoink 42:5a5ad23a4bb1 235 // printf(":: ");
dofydoink 42:5a5ad23a4bb1 236 //
dofydoink 42:5a5ad23a4bb1 237 // printf("%f\t",dblTheta);
dofydoink 42:5a5ad23a4bb1 238 // printf("%f\t",dblPhi);
dofydoink 42:5a5ad23a4bb1 239 // printf(":: ");
dofydoink 42:5a5ad23a4bb1 240 printf("\t");
dofydoink 42:5a5ad23a4bb1 241
dofydoink 42:5a5ad23a4bb1 242 printf("%f\t",dblX[1]);
dofydoink 42:5a5ad23a4bb1 243 printf("%f\t",dblX[0]);
dofydoink 42:5a5ad23a4bb1 244 printf("%f\t",dblX[2]);
dofydoink 42:5a5ad23a4bb1 245 printf(":: ");
dofydoink 42:5a5ad23a4bb1 246
dofydoink 42:5a5ad23a4bb1 247 // printf("%f\t",dblXdot[0]);
dofydoink 42:5a5ad23a4bb1 248 // printf("%f\t",dblXdot[2]);
dofydoink 42:5a5ad23a4bb1 249 // printf("%f\t",dblXdot[1]);
dofydoink 42:5a5ad23a4bb1 250 // printf(":: ");
dofydoink 42:5a5ad23a4bb1 251
dofydoink 42:5a5ad23a4bb1 252 printf("%f\t",dblS);
dofydoink 42:5a5ad23a4bb1 253 printf("%f\t",dblSdot);
dofydoink 42:5a5ad23a4bb1 254 printf("\r\n");
dofydoink 42:5a5ad23a4bb1 255 }
dofydoink 42:5a5ad23a4bb1 256 }
dofydoink 42:5a5ad23a4bb1 257 }
dofydoink 0:607bc887b6e0 258
dofydoink 42:5a5ad23a4bb1 259 double DeadZone(double var, double threshold, double max, double min){
dofydoink 42:5a5ad23a4bb1 260
dofydoink 42:5a5ad23a4bb1 261 double output = var;
dofydoink 42:5a5ad23a4bb1 262 output -= 0.5;
dofydoink 42:5a5ad23a4bb1 263 min -= 0.5;
dofydoink 42:5a5ad23a4bb1 264 max -= 0.5;
dofydoink 42:5a5ad23a4bb1 265
dofydoink 42:5a5ad23a4bb1 266 if(output > threshold){
dofydoink 42:5a5ad23a4bb1 267 output = (output - threshold);
dofydoink 42:5a5ad23a4bb1 268 output /= (max - threshold);
dofydoink 42:5a5ad23a4bb1 269 } else if(output < -1.0*threshold){
dofydoink 42:5a5ad23a4bb1 270 output = -1.0*(output + threshold);
dofydoink 42:5a5ad23a4bb1 271 output /= (threshold + min);
dofydoink 42:5a5ad23a4bb1 272 } else {
dofydoink 42:5a5ad23a4bb1 273 output = 0.0;
dofydoink 42:5a5ad23a4bb1 274 }
dofydoink 42:5a5ad23a4bb1 275
dofydoink 42:5a5ad23a4bb1 276 output = limDouble(output, -1.0, 1.0);
dofydoink 42:5a5ad23a4bb1 277 return output;
dofydoink 42:5a5ad23a4bb1 278 }
dofydoink 42:5a5ad23a4bb1 279
dofydoink 42:5a5ad23a4bb1 280 double ConvertToPWM(double target, double max){
dofydoink 42:5a5ad23a4bb1 281 double output;
dofydoink 42:5a5ad23a4bb1 282 output = target*0.8/max ;
dofydoink 42:5a5ad23a4bb1 283 output += 0.1;
dofydoink 42:5a5ad23a4bb1 284 return output;
dofydoink 42:5a5ad23a4bb1 285 }
WD40andTape 17:bbaf3e8440ad 286
dofydoink 42:5a5ad23a4bb1 287 void SetFrontDemand(double demandPos[], double demandVel[]){
dofydoink 42:5a5ad23a4bb1 288
dofydoink 42:5a5ad23a4bb1 289 double demandPos_abs[3];
dofydoink 42:5a5ad23a4bb1 290 double demandVel_abs[3];
dofydoink 42:5a5ad23a4bb1 291 double pwmPos_front[3];
dofydoink 42:5a5ad23a4bb1 292 double pwmVel_front[3];
dofydoink 42:5a5ad23a4bb1 293
dofydoink 42:5a5ad23a4bb1 294 for(int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 295 demandPos_abs[ii] = abs(demandPos[ii]);
dofydoink 42:5a5ad23a4bb1 296 demandVel_abs[ii] = abs(demandVel[ii]);
dofydoink 42:5a5ad23a4bb1 297 pwmPos_front[ii] = ConvertToPWM(demandPos_abs[ii], (double)MAX_ACTUATOR_LIMIT_MM);
dofydoink 42:5a5ad23a4bb1 298 pwmVel_front[ii] = ConvertToPWM(demandVel_abs[ii], (double)MAX_SPEED_MMPS);
dofydoink 42:5a5ad23a4bb1 299 pinPosOut[ii].write(pwmPos_front[ii]);
dofydoink 42:5a5ad23a4bb1 300 pinVelOut[ii].write(pwmVel_front[ii]);
dofydoink 42:5a5ad23a4bb1 301
dofydoink 42:5a5ad23a4bb1 302 }
dofydoink 42:5a5ad23a4bb1 303
dofydoink 42:5a5ad23a4bb1 304 /*
dofydoink 42:5a5ad23a4bb1 305 //print for debugging
dofydoink 42:5a5ad23a4bb1 306 printf("X: ");
dofydoink 42:5a5ad23a4bb1 307 for(int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 308 printf("%f\t",pwmPos_front[ii]);
dofydoink 42:5a5ad23a4bb1 309 }
dofydoink 42:5a5ad23a4bb1 310 printf("Xdot: ");
dofydoink 42:5a5ad23a4bb1 311 for(int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 312 printf("%f\t",pwmVel_front[ii]);
dofydoink 42:5a5ad23a4bb1 313 }
dofydoink 42:5a5ad23a4bb1 314 */
dofydoink 42:5a5ad23a4bb1 315 }
dofydoink 42:5a5ad23a4bb1 316
dofydoink 42:5a5ad23a4bb1 317 void SetDriveDemand(double drivePos, double driveVel){
dofydoink 42:5a5ad23a4bb1 318
dofydoink 42:5a5ad23a4bb1 319 double drivePos_abs;
dofydoink 42:5a5ad23a4bb1 320 double driveVel_abs;
dofydoink 42:5a5ad23a4bb1 321 double pwmPos_drive;
dofydoink 42:5a5ad23a4bb1 322 double pwmVel_drive;
dofydoink 42:5a5ad23a4bb1 323
dofydoink 42:5a5ad23a4bb1 324 drivePos_abs = abs(drivePos);
dofydoink 42:5a5ad23a4bb1 325 driveVel_abs = abs(driveVel);
dofydoink 42:5a5ad23a4bb1 326
dofydoink 42:5a5ad23a4bb1 327 pwmPos_drive = ConvertToPWM(drivePos_abs, MAX_DRIVE_SEGMENT_POSITION);
dofydoink 42:5a5ad23a4bb1 328 pwmVel_drive = ConvertToPWM(driveVel_abs, MAX_DRIVE_SEGMENT_SPEED);
dofydoink 42:5a5ad23a4bb1 329
dofydoink 42:5a5ad23a4bb1 330 pinPosOut[3].write(pwmPos_drive);
dofydoink 42:5a5ad23a4bb1 331 pinVelOut[3].write(pwmVel_drive);
dofydoink 42:5a5ad23a4bb1 332
dofydoink 42:5a5ad23a4bb1 333 /*
dofydoink 42:5a5ad23a4bb1 334 //print for debugging
dofydoink 42:5a5ad23a4bb1 335 printf("S:%f\tSdot:%f\t",pwmPos_drive,pwmVel_drive);
dofydoink 42:5a5ad23a4bb1 336 */
dofydoink 42:5a5ad23a4bb1 337 }
dofydoink 42:5a5ad23a4bb1 338
dofydoink 42:5a5ad23a4bb1 339 double GetMaxTravelTime(double startPositions[], double endPositions[], double speed){
dofydoink 42:5a5ad23a4bb1 340 double arT[3];
dofydoink 42:5a5ad23a4bb1 341 for(int ii = 0; ii< 3; ii++){
dofydoink 42:5a5ad23a4bb1 342 arT[ii] = endPositions[ii] - startPositions[ii];
dofydoink 42:5a5ad23a4bb1 343 arT[ii] /= speed;
dofydoink 42:5a5ad23a4bb1 344 arT[ii] = abs(arT[ii]);
dofydoink 42:5a5ad23a4bb1 345 }
dofydoink 42:5a5ad23a4bb1 346 double Tmax = 0.0;
dofydoink 42:5a5ad23a4bb1 347 for(int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 348 if( Tmax < arT[ii] ){
dofydoink 42:5a5ad23a4bb1 349 Tmax = arT[ii];
dofydoink 42:5a5ad23a4bb1 350 }
dofydoink 42:5a5ad23a4bb1 351 }
dofydoink 42:5a5ad23a4bb1 352 return Tmax;
dofydoink 42:5a5ad23a4bb1 353 }
dofydoink 42:5a5ad23a4bb1 354
dofydoink 42:5a5ad23a4bb1 355 void UpdateFrontSegPosition(double position[]){
dofydoink 42:5a5ad23a4bb1 356 for(int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 357 dblX[ii] = position[ii];
WD40andTape 17:bbaf3e8440ad 358 }
WD40andTape 17:bbaf3e8440ad 359 }
WD40andTape 17:bbaf3e8440ad 360
dofydoink 42:5a5ad23a4bb1 361 void GoToNeutralPose(double speed){
dofydoink 42:5a5ad23a4bb1 362 isBusy = 1;
dofydoink 42:5a5ad23a4bb1 363 double T_wait;
dofydoink 42:5a5ad23a4bb1 364 double arSpeed[3] = {speed, speed, speed};
dofydoink 42:5a5ad23a4bb1 365 double arPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
dofydoink 42:5a5ad23a4bb1 366 SetFrontDemand(arPosition, arSpeed);
dofydoink 42:5a5ad23a4bb1 367
dofydoink 42:5a5ad23a4bb1 368 //T_wait = 40.0 / speed;
dofydoink 42:5a5ad23a4bb1 369 T_wait = GetMaxTravelTime(dblX, arPosition, speed);
dofydoink 42:5a5ad23a4bb1 370 T_wait += 0.25;//add a little bit of time for security
dofydoink 42:5a5ad23a4bb1 371 //print for debug
dofydoink 42:5a5ad23a4bb1 372 printf("\r\nGoing to neutral. %f\r\n", T_wait);
dofydoink 42:5a5ad23a4bb1 373
dofydoink 42:5a5ad23a4bb1 374 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 375 UpdateFrontSegPosition(arPosition);
dofydoink 42:5a5ad23a4bb1 376 //isZero = 0;
dofydoink 42:5a5ad23a4bb1 377 isBusy = 0;
dofydoink 42:5a5ad23a4bb1 378 }
dofydoink 42:5a5ad23a4bb1 379
dofydoink 42:5a5ad23a4bb1 380 void GoToAnchorPose(double speed){
dofydoink 42:5a5ad23a4bb1 381 //reset front segment position
dofydoink 42:5a5ad23a4bb1 382 //isAnchor = 0;
dofydoink 42:5a5ad23a4bb1 383 isBusy = 1;
dofydoink 42:5a5ad23a4bb1 384 double T_wait;
dofydoink 42:5a5ad23a4bb1 385 double arSpeed[3] = {speed, speed, speed};
dofydoink 42:5a5ad23a4bb1 386 double arStartPosition[3];
dofydoink 42:5a5ad23a4bb1 387 //save starting position
dofydoink 42:5a5ad23a4bb1 388 for (int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 389 arStartPosition[ii] = dblX[ii];
dofydoink 42:5a5ad23a4bb1 390 }
dofydoink 42:5a5ad23a4bb1 391
dofydoink 42:5a5ad23a4bb1 392 double arAnchorPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
dofydoink 42:5a5ad23a4bb1 393
dofydoink 42:5a5ad23a4bb1 394 //Go to neutral
dofydoink 42:5a5ad23a4bb1 395 GoToNeutralPose(speed);
dofydoink 42:5a5ad23a4bb1 396
dofydoink 42:5a5ad23a4bb1 397 //select direction of anchor
dofydoink 42:5a5ad23a4bb1 398 arAnchorPosition[intAnchorDirection] = ANCHOR_POSITION;
dofydoink 42:5a5ad23a4bb1 399 intAnchorDirection++; //rotate next anchoring position
dofydoink 42:5a5ad23a4bb1 400 if(intAnchorDirection >= 3){
dofydoink 42:5a5ad23a4bb1 401 intAnchorDirection = 0;
dofydoink 42:5a5ad23a4bb1 402 }
dofydoink 42:5a5ad23a4bb1 403 //go to anchor position
dofydoink 42:5a5ad23a4bb1 404 SetFrontDemand(arAnchorPosition, arSpeed);
dofydoink 42:5a5ad23a4bb1 405 T_wait = GetMaxTravelTime(dblX, arAnchorPosition, speed);
dofydoink 42:5a5ad23a4bb1 406 T_wait += 0.25;//add a little bit of time for security
dofydoink 42:5a5ad23a4bb1 407 printf("Going to anchor. %f s\r\n", T_wait);
dofydoink 42:5a5ad23a4bb1 408 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 409
dofydoink 42:5a5ad23a4bb1 410 //update front segment position.
dofydoink 42:5a5ad23a4bb1 411 UpdateFrontSegPosition(arAnchorPosition);
dofydoink 42:5a5ad23a4bb1 412
dofydoink 42:5a5ad23a4bb1 413 //contract
dofydoink 42:5a5ad23a4bb1 414 SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED);
dofydoink 42:5a5ad23a4bb1 415 T_wait = dblS - HOME_DRIVE_POSITION;
dofydoink 42:5a5ad23a4bb1 416 T_wait /= SAFE_CONTRACT_SPEED;
dofydoink 42:5a5ad23a4bb1 417 printf("Contracting. %f s\r\n", T_wait);
dofydoink 42:5a5ad23a4bb1 418 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 419 //update drive segment position
dofydoink 42:5a5ad23a4bb1 420 dblS = HOME_DRIVE_POSITION;
dofydoink 42:5a5ad23a4bb1 421
dofydoink 42:5a5ad23a4bb1 422 //go back to neutral pose
dofydoink 42:5a5ad23a4bb1 423 GoToNeutralPose(speed);
dofydoink 42:5a5ad23a4bb1 424
dofydoink 42:5a5ad23a4bb1 425 //go back to starting position
dofydoink 42:5a5ad23a4bb1 426 SetFrontDemand(arStartPosition, arSpeed);
dofydoink 42:5a5ad23a4bb1 427 T_wait = GetMaxTravelTime(dblX, arStartPosition, speed);
dofydoink 42:5a5ad23a4bb1 428 T_wait += 0.25;//add a little bit of time for security
dofydoink 42:5a5ad23a4bb1 429 printf("Going to intial position. %f\r\n", T_wait);
dofydoink 42:5a5ad23a4bb1 430 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 431 //UpdateFrontSegPosition(arStartPosition);
dofydoink 42:5a5ad23a4bb1 432 for(int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 433 dblX[ii] = arStartPosition[ii];
dofydoink 42:5a5ad23a4bb1 434 }
dofydoink 42:5a5ad23a4bb1 435
dofydoink 42:5a5ad23a4bb1 436 //print for debug
dofydoink 42:5a5ad23a4bb1 437 isBusy = 0;
dofydoink 42:5a5ad23a4bb1 438 }
dofydoink 42:5a5ad23a4bb1 439
dofydoink 42:5a5ad23a4bb1 440
dofydoink 42:5a5ad23a4bb1 441 void GoToCustomPose(double position[], double speed){
dofydoink 42:5a5ad23a4bb1 442 //reset front segment position
dofydoink 42:5a5ad23a4bb1 443 //isBusy = 1;
dofydoink 42:5a5ad23a4bb1 444 double T_wait;
dofydoink 42:5a5ad23a4bb1 445 double arSpeed[3] = {speed, speed, speed};
dofydoink 42:5a5ad23a4bb1 446 for (int ii = 0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 447 position[ii] = limDouble(position[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM);
dofydoink 42:5a5ad23a4bb1 448 }
dofydoink 42:5a5ad23a4bb1 449 SetFrontDemand(position, arSpeed);
dofydoink 42:5a5ad23a4bb1 450
dofydoink 42:5a5ad23a4bb1 451 //T_wait = 40.0 / speed;
dofydoink 42:5a5ad23a4bb1 452 T_wait = GetMaxTravelTime(dblX, position, speed);
dofydoink 42:5a5ad23a4bb1 453 T_wait += 0.25;//add a little bit of time for security
dofydoink 42:5a5ad23a4bb1 454 //print for debug
dofydoink 42:5a5ad23a4bb1 455 printf("\r\nGoing to custom pose. T_wait: %f s \r\n", T_wait);
dofydoink 42:5a5ad23a4bb1 456 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 457 UpdateFrontSegPosition(position);
dofydoink 42:5a5ad23a4bb1 458 printf("Done\r\n");
dofydoink 42:5a5ad23a4bb1 459 //isBusy = 0;
dofydoink 42:5a5ad23a4bb1 460 }
dofydoink 42:5a5ad23a4bb1 461
dofydoink 42:5a5ad23a4bb1 462
dofydoink 42:5a5ad23a4bb1 463 void AllGoTo(double position, double speed){
dofydoink 42:5a5ad23a4bb1 464 //reset front segment position
dofydoink 42:5a5ad23a4bb1 465 //isBusy = 1;
dofydoink 42:5a5ad23a4bb1 466 double T_wait;
dofydoink 42:5a5ad23a4bb1 467 double arSpeed[3] = {speed, speed, speed};
dofydoink 42:5a5ad23a4bb1 468 position = limDouble(position, 0.0, (double)MAX_ACTUATOR_LIMIT_MM);
dofydoink 42:5a5ad23a4bb1 469
dofydoink 42:5a5ad23a4bb1 470 double arPosition[3] = {position, position, position};
dofydoink 42:5a5ad23a4bb1 471 SetFrontDemand(arPosition, arSpeed);
dofydoink 42:5a5ad23a4bb1 472
dofydoink 42:5a5ad23a4bb1 473 //T_wait = 40.0 / speed;
dofydoink 42:5a5ad23a4bb1 474 T_wait = GetMaxTravelTime(dblX, arPosition, speed);
dofydoink 42:5a5ad23a4bb1 475 T_wait += 0.25;//add a little bit of time for security
dofydoink 42:5a5ad23a4bb1 476 //print for debug
dofydoink 42:5a5ad23a4bb1 477 printf("\r\nMoving all to %f mm. T_wait: %f\r\n",position, T_wait);
dofydoink 42:5a5ad23a4bb1 478
dofydoink 42:5a5ad23a4bb1 479 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 480 UpdateFrontSegPosition(arPosition);
dofydoink 42:5a5ad23a4bb1 481 printf("Done\r\n");
dofydoink 42:5a5ad23a4bb1 482 //isBusy = 0;
dofydoink 42:5a5ad23a4bb1 483 }
dofydoink 42:5a5ad23a4bb1 484
dofydoink 42:5a5ad23a4bb1 485 void FlushSyringes(int repetitions){
dofydoink 42:5a5ad23a4bb1 486 //isBusy = 1;
dofydoink 42:5a5ad23a4bb1 487 printf("\r\n\r\n:::Flushing Syringes:::\r\n");
dofydoink 42:5a5ad23a4bb1 488 pinFlushOut = 1;
dofydoink 42:5a5ad23a4bb1 489 wait(1.0);
dofydoink 42:5a5ad23a4bb1 490 for(int ii = 0; ii<repetitions; ii++){
dofydoink 42:5a5ad23a4bb1 491 AllGoTo(40.0,20.0);
dofydoink 42:5a5ad23a4bb1 492 wait(0.5);
dofydoink 42:5a5ad23a4bb1 493 AllGoTo(0.0,2.0);
dofydoink 42:5a5ad23a4bb1 494 wait(0.5);
dofydoink 42:5a5ad23a4bb1 495 }
dofydoink 42:5a5ad23a4bb1 496 pinFlushOut = 0;
dofydoink 42:5a5ad23a4bb1 497 printf("Done\r\n");
dofydoink 42:5a5ad23a4bb1 498 //isBusy = 0;
dofydoink 42:5a5ad23a4bb1 499 }
dofydoink 42:5a5ad23a4bb1 500
dofydoink 42:5a5ad23a4bb1 501 void FlushSegment(int repetitions){
dofydoink 42:5a5ad23a4bb1 502 //isBusy = 1;
dofydoink 42:5a5ad23a4bb1 503 printf("\r\n\r\n:::Flushing Segment:::\r\n");
dofydoink 42:5a5ad23a4bb1 504 FlushSyringes(1);//ensure syringes are flushed.
dofydoink 42:5a5ad23a4bb1 505 double arZero[3] = {0.0, 0.0, 0.0};
dofydoink 42:5a5ad23a4bb1 506
dofydoink 42:5a5ad23a4bb1 507 double flushPosition = 30.0;
dofydoink 42:5a5ad23a4bb1 508
dofydoink 42:5a5ad23a4bb1 509 double arFlushPose1[3] = {flushPosition, 0.0, 0.0};
dofydoink 42:5a5ad23a4bb1 510 double arFlushPose2[3] = {0.0, flushPosition, 0.0};
dofydoink 42:5a5ad23a4bb1 511 double arFlushPose3[3] = {0.0, 0.0, flushPosition};
dofydoink 42:5a5ad23a4bb1 512 for(int ii = 0; ii<repetitions; ii++){
dofydoink 42:5a5ad23a4bb1 513 //flush first chamber
dofydoink 42:5a5ad23a4bb1 514 printf("Flushing first chamber\r\n");
dofydoink 42:5a5ad23a4bb1 515 GoToCustomPose(arFlushPose1, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 516 AllGoTo(0.0, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 517
dofydoink 42:5a5ad23a4bb1 518 //flush second chamber
dofydoink 42:5a5ad23a4bb1 519 printf("Flushing second chamber\r\n");
dofydoink 42:5a5ad23a4bb1 520 GoToCustomPose(arFlushPose2, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 521 AllGoTo(0.0, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 522
dofydoink 42:5a5ad23a4bb1 523 //flush third chamber
dofydoink 42:5a5ad23a4bb1 524 printf("Flushing third chamber\r\n");
dofydoink 42:5a5ad23a4bb1 525 GoToCustomPose(arFlushPose3, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 526 AllGoTo(0.0, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 527
dofydoink 42:5a5ad23a4bb1 528 FlushSyringes(1);//flush syringes
dofydoink 42:5a5ad23a4bb1 529 }
dofydoink 42:5a5ad23a4bb1 530 printf("Done\r\n");
dofydoink 42:5a5ad23a4bb1 531 //isBusy = 0;
WD40andTape 17:bbaf3e8440ad 532 }
dofydoink 0:607bc887b6e0 533
dofydoink 42:5a5ad23a4bb1 534 void FlushTubing(int repetitions){
dofydoink 42:5a5ad23a4bb1 535 //isBusy = 1;
dofydoink 42:5a5ad23a4bb1 536 printf("\r\n\r\n:::Flushing Tubing:::\r\n");
dofydoink 42:5a5ad23a4bb1 537 for (int ii = 0; ii<repetitions; ii++){
dofydoink 42:5a5ad23a4bb1 538 AllGoTo(40.0,15.0);
dofydoink 42:5a5ad23a4bb1 539 pinFlushOut = 1;
dofydoink 42:5a5ad23a4bb1 540 wait(1.0);
dofydoink 42:5a5ad23a4bb1 541 AllGoTo(0.0, 2.0);
dofydoink 42:5a5ad23a4bb1 542 pinFlushOut = 0;
dofydoink 42:5a5ad23a4bb1 543 wait(0.5);
dofydoink 42:5a5ad23a4bb1 544 }
dofydoink 42:5a5ad23a4bb1 545 }
WD40andTape 6:f0a18e28a322 546
dofydoink 42:5a5ad23a4bb1 547
dofydoink 42:5a5ad23a4bb1 548 void DebugMode(){
dofydoink 42:5a5ad23a4bb1 549 isBusy = 1;
dofydoink 42:5a5ad23a4bb1 550 printf("\r\nEntering debug mode. Going to zero.\r\n");
dofydoink 42:5a5ad23a4bb1 551 double zeroPos[3] = {0.0, 0.0, 0.0};
dofydoink 42:5a5ad23a4bb1 552 double safeVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS};
WD40andTape 17:bbaf3e8440ad 553
dofydoink 42:5a5ad23a4bb1 554
dofydoink 42:5a5ad23a4bb1 555 SetFrontDemand(zeroPos, safeVel);
dofydoink 42:5a5ad23a4bb1 556 SetDriveDemand(0.0, SAFE_CONTRACT_SPEED);
dofydoink 42:5a5ad23a4bb1 557 double T_wait = GetMaxTravelTime(dblX, zeroPos, SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 558 T_wait += 0.25;
dofydoink 42:5a5ad23a4bb1 559 wait(T_wait);
dofydoink 42:5a5ad23a4bb1 560 UpdateFrontSegPosition(zeroPos);
dofydoink 42:5a5ad23a4bb1 561 printf("Done\r\nPlease enter command\r\n");
dofydoink 42:5a5ad23a4bb1 562
dofydoink 42:5a5ad23a4bb1 563 int countSyringeFlush = 0;
dofydoink 42:5a5ad23a4bb1 564 int countSegmentFlush = 0;
dofydoink 42:5a5ad23a4bb1 565 int countTubingFlush = 0;
WD40andTape 4:303584310071 566
dofydoink 42:5a5ad23a4bb1 567 while(isDebug){
dofydoink 42:5a5ad23a4bb1 568 isBusy = 1;
dofydoink 42:5a5ad23a4bb1 569 //printf("x\r\n");
dofydoink 42:5a5ad23a4bb1 570
dofydoink 42:5a5ad23a4bb1 571 if( pinZeroIn.read() ){
dofydoink 42:5a5ad23a4bb1 572 //printf("zero%d\r\n",countSyringeFlush);
dofydoink 42:5a5ad23a4bb1 573
dofydoink 42:5a5ad23a4bb1 574 if(countSyringeFlush == 0){
dofydoink 42:5a5ad23a4bb1 575 printf("\r\n Syringe Flush Selected\r\n");
dofydoink 42:5a5ad23a4bb1 576 }
dofydoink 42:5a5ad23a4bb1 577 countSyringeFlush++;
dofydoink 42:5a5ad23a4bb1 578 countSegmentFlush = 0;
dofydoink 42:5a5ad23a4bb1 579 countTubingFlush = 0;
dofydoink 42:5a5ad23a4bb1 580 if(countSyringeFlush > 50){
dofydoink 42:5a5ad23a4bb1 581 FlushSyringes(4);
dofydoink 42:5a5ad23a4bb1 582 countSyringeFlush = 0;
WD40andTape 33:9877ca32e43c 583 }
dofydoink 42:5a5ad23a4bb1 584 //printf("%d\r\n", countSyringeFlush);
dofydoink 42:5a5ad23a4bb1 585
dofydoink 42:5a5ad23a4bb1 586 }else {
dofydoink 42:5a5ad23a4bb1 587 countSyringeFlush = 0;
dofydoink 42:5a5ad23a4bb1 588 }
dofydoink 42:5a5ad23a4bb1 589
dofydoink 42:5a5ad23a4bb1 590 if( pinSucIn.read() ){
dofydoink 42:5a5ad23a4bb1 591 //printf("suc%d\r\n", countSegmentFlush);
dofydoink 42:5a5ad23a4bb1 592
dofydoink 42:5a5ad23a4bb1 593 if(countSegmentFlush == 0){
dofydoink 42:5a5ad23a4bb1 594 printf("\r\n Segment Flush Selected\r\n");
dofydoink 42:5a5ad23a4bb1 595 }
dofydoink 42:5a5ad23a4bb1 596 countSyringeFlush = 0;
dofydoink 42:5a5ad23a4bb1 597 countSegmentFlush++;
dofydoink 42:5a5ad23a4bb1 598 countTubingFlush = 0;
dofydoink 42:5a5ad23a4bb1 599 if(countSegmentFlush > 50){
dofydoink 42:5a5ad23a4bb1 600 FlushSegment(2);
dofydoink 42:5a5ad23a4bb1 601 countSegmentFlush = 0;
dofydoink 42:5a5ad23a4bb1 602 }
dofydoink 42:5a5ad23a4bb1 603 }else {
dofydoink 42:5a5ad23a4bb1 604 countSegmentFlush = 0;
dofydoink 42:5a5ad23a4bb1 605 }
dofydoink 42:5a5ad23a4bb1 606
dofydoink 42:5a5ad23a4bb1 607 if( pinWashIn.read() ){
dofydoink 42:5a5ad23a4bb1 608 //printf("wash%d\r\n", countTubingFlush);
dofydoink 42:5a5ad23a4bb1 609 if(countTubingFlush == 0){
dofydoink 42:5a5ad23a4bb1 610 printf("\r\n Tubing Flush Selected\r\n");
dofydoink 42:5a5ad23a4bb1 611 }
dofydoink 42:5a5ad23a4bb1 612 countSyringeFlush = 0;
dofydoink 42:5a5ad23a4bb1 613 countSegmentFlush = 0;
dofydoink 42:5a5ad23a4bb1 614 countTubingFlush++;
dofydoink 42:5a5ad23a4bb1 615 if(countTubingFlush > 50){
dofydoink 42:5a5ad23a4bb1 616 FlushTubing(2);
dofydoink 42:5a5ad23a4bb1 617 countTubingFlush = 0;
dofydoink 42:5a5ad23a4bb1 618 }
dofydoink 42:5a5ad23a4bb1 619 }else {
dofydoink 42:5a5ad23a4bb1 620 countTubingFlush = 0;
WD40andTape 33:9877ca32e43c 621 }
dofydoink 42:5a5ad23a4bb1 622
dofydoink 42:5a5ad23a4bb1 623 wait(0.1);
dofydoink 42:5a5ad23a4bb1 624 }
dofydoink 42:5a5ad23a4bb1 625
dofydoink 42:5a5ad23a4bb1 626 GoToNeutralPose(SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 627 //reset front segment position
dofydoink 42:5a5ad23a4bb1 628 dblAlpha = 0.0;
dofydoink 42:5a5ad23a4bb1 629 dblBeta = 0.0;
dofydoink 42:5a5ad23a4bb1 630 printf("\r\nManual control enabled\r\n");
dofydoink 42:5a5ad23a4bb1 631 isBusy = 0;
dofydoink 42:5a5ad23a4bb1 632 }
dofydoink 42:5a5ad23a4bb1 633
dofydoink 42:5a5ad23a4bb1 634 void ReadHHC(){
dofydoink 42:5a5ad23a4bb1 635 printf("Homing. Please wait...\r\n");
dofydoink 42:5a5ad23a4bb1 636
dofydoink 42:5a5ad23a4bb1 637
dofydoink 42:5a5ad23a4bb1 638 double startingPos[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
dofydoink 42:5a5ad23a4bb1 639 double startingVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS};
dofydoink 42:5a5ad23a4bb1 640 SetFrontDemand(startingPos, startingVel);
dofydoink 42:5a5ad23a4bb1 641 SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED);
dofydoink 42:5a5ad23a4bb1 642 wait(5);
dofydoink 42:5a5ad23a4bb1 643 printf("Done.\r\n");
dofydoink 42:5a5ad23a4bb1 644 threadSendFeedback.start(SendFeedback);
dofydoink 42:5a5ad23a4bb1 645 tickerSendFeedback.attach(&signalSendFeedback, 1/(float)SENSOR_FEEDBACK_HZ);
dofydoink 42:5a5ad23a4bb1 646 while(1){
dofydoink 42:5a5ad23a4bb1 647 semReadHHC.wait();
dofydoink 42:5a5ad23a4bb1 648
dofydoink 42:5a5ad23a4bb1 649 if(isZero){
dofydoink 42:5a5ad23a4bb1 650 GoToNeutralPose(SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 651 //reset front segment position
dofydoink 42:5a5ad23a4bb1 652 dblAlpha = 0.0;
dofydoink 42:5a5ad23a4bb1 653 dblBeta = 0.0;
dofydoink 42:5a5ad23a4bb1 654 isZero = 0;
dofydoink 42:5a5ad23a4bb1 655 printf("Done\r\n");
dofydoink 42:5a5ad23a4bb1 656 }
dofydoink 42:5a5ad23a4bb1 657
dofydoink 42:5a5ad23a4bb1 658 if(isCycle){
dofydoink 42:5a5ad23a4bb1 659 GoToAnchorPose(SAFE_SPEED_MMpS);
dofydoink 42:5a5ad23a4bb1 660 isCycle = 0;
dofydoink 42:5a5ad23a4bb1 661 printf("Done\r\n");
dofydoink 42:5a5ad23a4bb1 662 }
dofydoink 42:5a5ad23a4bb1 663 if(isDebug){
dofydoink 42:5a5ad23a4bb1 664 DebugMode();
dofydoink 42:5a5ad23a4bb1 665 }
dofydoink 42:5a5ad23a4bb1 666
dofydoink 42:5a5ad23a4bb1 667 if(!isSteerLock){
dofydoink 42:5a5ad23a4bb1 668 //read Joystick sensors
dofydoink 42:5a5ad23a4bb1 669 double dblJSX = pinJSX.read();
dofydoink 42:5a5ad23a4bb1 670 double dblJSY = pinJSY.read();
dofydoink 42:5a5ad23a4bb1 671
dofydoink 42:5a5ad23a4bb1 672 dblJSX = DeadZone(dblJSX, 0.07, 0.85, 0.15);
dofydoink 42:5a5ad23a4bb1 673 dblJSY = DeadZone(dblJSY, 0.07, 0.85, 0.15);
dofydoink 42:5a5ad23a4bb1 674
dofydoink 42:5a5ad23a4bb1 675 if(pinWashIn.read()){
dofydoink 42:5a5ad23a4bb1 676 pinWashOut = 0;
dofydoink 42:5a5ad23a4bb1 677 }else{
dofydoink 42:5a5ad23a4bb1 678 pinWashOut = 1;
dofydoink 42:5a5ad23a4bb1 679 }
dofydoink 42:5a5ad23a4bb1 680
dofydoink 42:5a5ad23a4bb1 681 //convert to angular velocity for front segment
dofydoink 42:5a5ad23a4bb1 682
dofydoink 42:5a5ad23a4bb1 683 //Find changes in angle
dofydoink 42:5a5ad23a4bb1 684 dblDeltaAlpha = dblJSX * x_axis_sign * dblSensitivity * MAX_STEER_SPEED_RADpS / HHC_READ_FREQ_HZ;
dofydoink 42:5a5ad23a4bb1 685 dblDeltaBeta = dblJSY*dblSensitivity*MAX_STEER_SPEED_RADpS/HHC_READ_FREQ_HZ;
dofydoink 42:5a5ad23a4bb1 686 //convert to speeds in [rad/s]
dofydoink 42:5a5ad23a4bb1 687 dblAlphaDot = dblDeltaAlpha*HHC_READ_FREQ_HZ;
dofydoink 42:5a5ad23a4bb1 688 dblBetaDot = dblDeltaBeta*HHC_READ_FREQ_HZ;
dofydoink 42:5a5ad23a4bb1 689
dofydoink 42:5a5ad23a4bb1 690 //calculate required velocity based on current possition and new inputs.
dofydoink 42:5a5ad23a4bb1 691 dblPhi = atan2(dblBeta,dblAlpha);
dofydoink 42:5a5ad23a4bb1 692 double dblPhiDot = dblAlpha * dblBetaDot - dblBeta * dblAlphaDot;
dofydoink 42:5a5ad23a4bb1 693 dblPhiDot /= (dblAlpha * dblAlpha + dblBeta*dblBeta);
dofydoink 42:5a5ad23a4bb1 694 if( fabs( cos(dblPhi) ) >= 0.7106781 ){
dofydoink 42:5a5ad23a4bb1 695 dblTheta = dblAlpha / cos(dblPhi);
dofydoink 42:5a5ad23a4bb1 696 double dblThetaDot = dblAlphaDot * cos(dblPhi) + dblAlpha * dblPhiDot * sin(dblPhi);
dofydoink 42:5a5ad23a4bb1 697 dblThetaDot /= cos(dblPhi)*cos(dblPhi);
dofydoink 42:5a5ad23a4bb1 698 double dblPsi[3];
dofydoink 42:5a5ad23a4bb1 699 for (int ii =0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 700 dblPsi[ii] = dblPhi - ii*2*PI/3;
dofydoink 42:5a5ad23a4bb1 701 if(dblBeta == 0){
dofydoink 42:5a5ad23a4bb1 702 dblXdot[ii] = ACT_CONV * K_theta*( cos(dblPsi[ii]) + d_theta);
dofydoink 42:5a5ad23a4bb1 703 dblXdot[ii] *= ( dblAlphaDot - dblBetaDot * sin(dblPsi[ii]) ) / cos(dblPhi);
WD40andTape 32:8c59c536a2a6 704 }
dofydoink 42:5a5ad23a4bb1 705 else{
dofydoink 42:5a5ad23a4bb1 706 dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot);
dofydoink 42:5a5ad23a4bb1 707 }
dofydoink 42:5a5ad23a4bb1 708 }
dofydoink 42:5a5ad23a4bb1 709 }
dofydoink 42:5a5ad23a4bb1 710 else {
dofydoink 42:5a5ad23a4bb1 711 dblTheta = dblBeta / sin(dblPhi);
dofydoink 42:5a5ad23a4bb1 712 double dblThetaDot = dblBetaDot * sin(dblPhi) + dblBeta * dblPhiDot * cos(dblPhi);
dofydoink 42:5a5ad23a4bb1 713 dblThetaDot /= sin(dblPhi)*sin(dblPhi);
dofydoink 42:5a5ad23a4bb1 714 double dblPsi[3];
dofydoink 42:5a5ad23a4bb1 715 for (int ii =0; ii<3; ii++){
dofydoink 42:5a5ad23a4bb1 716 dblPsi[ii] = dblPhi - ii*2*PI/3;
dofydoink 42:5a5ad23a4bb1 717 if(dblAlpha == 0){
dofydoink 42:5a5ad23a4bb1 718 dblXdot[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta);
dofydoink 42:5a5ad23a4bb1 719 dblXdot[ii] *= ( dblBetaDot - dblAlphaDot * sin(dblPsi[ii]) ) / sin(dblPhi);
WD40andTape 31:08cb04eb75fc 720 }
dofydoink 42:5a5ad23a4bb1 721 else{
dofydoink 42:5a5ad23a4bb1 722 dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta ) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot);
WD40andTape 31:08cb04eb75fc 723 }
WD40andTape 29:10a5cf37a875 724 }
dofydoink 42:5a5ad23a4bb1 725 }
dofydoink 42:5a5ad23a4bb1 726 //calculate new angles for next front segment position
dofydoink 42:5a5ad23a4bb1 727 dblAlpha += dblDeltaAlpha; // update new alpha postion
dofydoink 42:5a5ad23a4bb1 728 dblBeta += dblDeltaBeta; // update new beta position
dofydoink 42:5a5ad23a4bb1 729
dofydoink 42:5a5ad23a4bb1 730 dblAlpha = limDouble(dblAlpha, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD);
dofydoink 42:5a5ad23a4bb1 731 dblBeta = limDouble(dblBeta, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD);
dofydoink 42:5a5ad23a4bb1 732 dblPhi = atan2(dblBeta,dblAlpha); // update new phi position
dofydoink 42:5a5ad23a4bb1 733 if( fabs( cos(dblPhi) ) >= 0.7106781 ){
dofydoink 42:5a5ad23a4bb1 734 dblTheta = dblAlpha / cos(dblPhi);//calculate new theta value
dofydoink 42:5a5ad23a4bb1 735 }
dofydoink 42:5a5ad23a4bb1 736 else {
dofydoink 42:5a5ad23a4bb1 737 dblTheta = dblBeta / sin(dblPhi);
dofydoink 42:5a5ad23a4bb1 738 }
dofydoink 42:5a5ad23a4bb1 739 dblTheta = limDouble(dblTheta, 0.0, (double)MAX_THETA_RAD ); //limit angle
dofydoink 42:5a5ad23a4bb1 740 //update alpha and beta if limit has occured
dofydoink 42:5a5ad23a4bb1 741 dblAlpha = dblTheta *cos(dblPhi);
dofydoink 42:5a5ad23a4bb1 742 dblBeta = dblTheta *sin(dblPhi);
dofydoink 42:5a5ad23a4bb1 743
dofydoink 42:5a5ad23a4bb1 744 double dblPsi[3];
dofydoink 42:5a5ad23a4bb1 745 for (int ii = 0; ii <3; ii++) {//calculate new acuator positions
dofydoink 42:5a5ad23a4bb1 746 dblPsi[ii] = dblPhi - ii*2*PI/3;
dofydoink 42:5a5ad23a4bb1 747 dblX[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta) * dblTheta;
dofydoink 42:5a5ad23a4bb1 748 //add offset
dofydoink 42:5a5ad23a4bb1 749 dblX[ii] += ACTUATOR_OFFSET;
dofydoink 42:5a5ad23a4bb1 750
dofydoink 42:5a5ad23a4bb1 751 //limit positions
dofydoink 42:5a5ad23a4bb1 752 dblX[ii] = limDouble( dblX[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM );
dofydoink 42:5a5ad23a4bb1 753 }
dofydoink 42:5a5ad23a4bb1 754 //calculate new drive segment position
dofydoink 42:5a5ad23a4bb1 755 //read drive segment buttons
dofydoink 42:5a5ad23a4bb1 756 //double dblDriveStep = 0.4;
dofydoink 42:5a5ad23a4bb1 757
dofydoink 42:5a5ad23a4bb1 758 //printf("pin:%d\t", pinFwd.read());
dofydoink 42:5a5ad23a4bb1 759
dofydoink 42:5a5ad23a4bb1 760 if(pinFwd.read()){
dofydoink 42:5a5ad23a4bb1 761 if(!pinRev.read()){
dofydoink 42:5a5ad23a4bb1 762 dblSdot = SAFE_CONTRACT_SPEED;
dofydoink 42:5a5ad23a4bb1 763 } else{
dofydoink 42:5a5ad23a4bb1 764 dblSdot = 0.0;
WD40andTape 19:e5acb2183d4e 765 }
WD40andTape 31:08cb04eb75fc 766 } else {
dofydoink 42:5a5ad23a4bb1 767 if(pinRev.read()){
dofydoink 42:5a5ad23a4bb1 768 dblSdot = -1.0*SAFE_CONTRACT_SPEED;
dofydoink 42:5a5ad23a4bb1 769 } else{
dofydoink 42:5a5ad23a4bb1 770 dblSdot = 0.0;
dofydoink 42:5a5ad23a4bb1 771 }
WD40andTape 19:e5acb2183d4e 772 }
dofydoink 42:5a5ad23a4bb1 773
dofydoink 42:5a5ad23a4bb1 774 dblS += dblSdot/HHC_READ_FREQ_HZ;
dofydoink 42:5a5ad23a4bb1 775 //limit extension (uses word position, but actually refers to pressure)
dofydoink 42:5a5ad23a4bb1 776 dblS = limDouble( dblS, 0.0, (double)MAX_EXTENSION_PRESSURE );
dofydoink 42:5a5ad23a4bb1 777
dofydoink 42:5a5ad23a4bb1 778 // printf("S:%f\tSdot:%f\t", dblS, dblSdot);
dofydoink 42:5a5ad23a4bb1 779 //Send signals to actuators.
dofydoink 42:5a5ad23a4bb1 780 SetFrontDemand(dblX, dblXdot);//front segment
dofydoink 42:5a5ad23a4bb1 781 SetDriveDemand(dblS, dblSdot);//drive segment
dofydoink 42:5a5ad23a4bb1 782
dofydoink 42:5a5ad23a4bb1 783
dofydoink 11:7029367a1840 784 }
WD40andTape 6:f0a18e28a322 785 }
WD40andTape 24:bc852aa89e7a 786 }
WD40andTape 24:bc852aa89e7a 787
dofydoink 42:5a5ad23a4bb1 788 void SetUp(){
dofydoink 42:5a5ad23a4bb1 789 for(int ii = 0; ii<N_CHANNELS; ii++){
dofydoink 42:5a5ad23a4bb1 790 pinPosOut[ii].write(0.0);
dofydoink 42:5a5ad23a4bb1 791 pinVelOut[ii].write(0.0);
dofydoink 42:5a5ad23a4bb1 792 }
WD40andTape 3:c83291bf9fd2 793 }
dofydoink 0:607bc887b6e0 794
WD40andTape 13:a373dfc57b89 795 int main() {
dofydoink 42:5a5ad23a4bb1 796 pinThirdPump = 1;
dofydoink 42:5a5ad23a4bb1 797 pinJetOut = 1;
dofydoink 42:5a5ad23a4bb1 798 pinWashOut = 1;
WD40andTape 7:5b6a2cefbf3b 799 pc.baud(BAUD_RATE);
dofydoink 42:5a5ad23a4bb1 800 for(int ii = 0; ii<N_CHANNELS; ii++){
dofydoink 42:5a5ad23a4bb1 801 pinPosOut[ii].period_ms(2.0);
dofydoink 42:5a5ad23a4bb1 802 pinVelOut[ii].period_ms(2.0);
dofydoink 42:5a5ad23a4bb1 803 }
dofydoink 42:5a5ad23a4bb1 804 wait(1.0);
dofydoink 42:5a5ad23a4bb1 805 printf("ML engage. Compiled at %s\r\n",__TIME__);
dofydoink 42:5a5ad23a4bb1 806
dofydoink 42:5a5ad23a4bb1 807
dofydoink 42:5a5ad23a4bb1 808 //set up support function interrupts
dofydoink 42:5a5ad23a4bb1 809 //Rising
WD40andTape 9:cd3607ba5643 810
dofydoink 42:5a5ad23a4bb1 811 pinZeroIn.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 812 pinCycleIn.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 813 pinWashIn.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 814 pinGasIn.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 815 pinSucIn.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 816 pinFwd.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 817 pinRev.mode(PullNone);
dofydoink 42:5a5ad23a4bb1 818 pinFlushIn.mode(PullDown);
dofydoink 42:5a5ad23a4bb1 819 pinDebugIn.mode(PullDown);
dofydoink 42:5a5ad23a4bb1 820
dofydoink 42:5a5ad23a4bb1 821 pinZeroIn.rise(&ZeroActivate);
dofydoink 42:5a5ad23a4bb1 822 pinCycleIn.rise(&CycleActivate);
dofydoink 42:5a5ad23a4bb1 823 pinFlushIn.rise(&FlushActivate);
dofydoink 42:5a5ad23a4bb1 824 pinSucIn.rise(&SucActivate);
dofydoink 42:5a5ad23a4bb1 825 pinGasIn.rise(&GasActivate);
dofydoink 42:5a5ad23a4bb1 826 pinDebugIn.rise(&DebugActivate);
dofydoink 42:5a5ad23a4bb1 827 //falling
dofydoink 42:5a5ad23a4bb1 828 pinZeroIn.fall(&ZeroStop);
dofydoink 42:5a5ad23a4bb1 829 pinCycleIn.fall(&CycleStop);
dofydoink 42:5a5ad23a4bb1 830 pinFlushIn.fall(&FlushStop);
dofydoink 42:5a5ad23a4bb1 831 pinSucIn.fall(&SucStop);
dofydoink 42:5a5ad23a4bb1 832 pinGasIn.fall(&GasStop);
dofydoink 42:5a5ad23a4bb1 833 pinDebugIn.fall(&DebugStop);
dofydoink 42:5a5ad23a4bb1 834 threadReadHHC.start(ReadHHC);
WD40andTape 7:5b6a2cefbf3b 835
dofydoink 0:607bc887b6e0 836
dofydoink 42:5a5ad23a4bb1 837 tickerReadHHC.attach(&signalReadHHC, 1/(float)HHC_READ_FREQ_HZ); // set up
dofydoink 42:5a5ad23a4bb1 838
dofydoink 42:5a5ad23a4bb1 839
WD40andTape 1:2a43cf183a62 840 while(1) {
WD40andTape 1:2a43cf183a62 841 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 842 }
WD40andTape 7:5b6a2cefbf3b 843 }