/******************************************************* Name: Main.c Date: 12/17/2018 Created By: Shane King Description: Main source code file for dsPIC stumble targeting algorithm *****************************************************/ #include "Defs.h" #include "Globals.h" #include #include #include /* These are function prototypes, which define the proper usage of functions located in other files */ extern void Ecan1Init(void); // Located in ECAN1Config.c extern void CanSend(UINT16 ID); // Located in CanSend.c extern void CanSendAll(void); // Located in CanSend.c extern void CanSend0x20(void); extern void CanSend0x21(void); extern void CanSend0x22(void); extern void CanSend0x23(void); extern int CanParse(void); extern void InitOsc(void); // Located in Init.c extern void InitGPIO(void); // Located in Init.c extern void InitGlobs(void); // Located in Init.c extern void InitEcan1(void); // Located in Init.c extern void TimersInit(void); // Located in Timers.c extern void ADCInit(void); extern void PWMInit(void); UINT16 ADCRead(UINT16 chan); void DataAcquisition(void); // Located in Main.c void BlockRelease(void); // Located in Main.c void SimulinkOutput(void); // Located in Main.c void MeanZero(void); // Located in Main.c void Calibration(void); // Located in Main.c void Restart(void); // Located in Main.c int main(void) { // Call routines to initialize various functions InitOsc(); // Located in Init.c InitGPIO(); // Located in Init.c InitGlobs(); // Located in Init.c TimersInit(); // Located in Timers.c Ecan1Init(); // Located in ECAN1Config.c ADCInit(); // Initialize ADC PWMInit(); // Initialize PWM /* Set register bits that control global interrupts. * See section 7 of dsPIC datasheet */ INTCON1bits.NSTDIS = 0; // Disable nested interrupts SRbits.IPL = 0; // Set interrupt priority, see section 7.3 //Main Loop while (1) { UINT16 i = 0; //Initializes counter if (IFS0bits.T1IF == 1) { // TMR1 has ticked (1 kHz timer) IFS0bits.T1IF = 0; // reset TMR1 if (restartCheck == 1) Restart(); // Restarts system if Simulink has stopped if (meanCheck == 1) MeanZero(); // Zeros treadmill kinetic signals if (meanCheck == 0) DataAcquisition(); // Collects treadmill kinetic signals if (caliCheck == 1 || caliCheck == 2) Calibration(); // Calibrates system to determine entry time and position of block if (stumbleCheck == 1) BlockRelease(); //Block release algorithm SimulinkOutput(); //Simulink output via CAN CANData[3] = 0; //Initialize input CAN variable CANData3[2] = 0; //Initialize input CAN variable CanParse(); //Reads data from CAN bus from Simulink runTime = CANData[3]; //Collect current run (clock) time from Simulink diffTime = runTime - pastTime; //Compare current run time to past run time to determine is Simulink input has stopped if (diffTime == 0) restartCount++; //If difference equals zero Simulink input has stopped else restartCount = 0; //If not reset restart counter if (restartCount == restartTime) restartCheck = 1; //If restart counter reaches threshold then set restart flag to high for (i = 0; i < nCAN; i++) gData4[i] = 0; //Initialize output CAN variable gData4[0] = ltmLength; //Output left belt block entry position gData4[1] = llatencyTime; //Output left belt block entry time gData4[2] = rtmLength; //Output right bel block entry position gData4[3] = rlatencyTime; //Output right belt block entry time CanSend0x23(); //Send variables via CAN bus pastTime = runTime; //Set previous run time equal to current run time } } } void DataAcquisition(void) { //This function aquires data from the force-instrumented treadmill const int h = -25; //Belt surface to treadmill origin offset UINT16 lfpData[5]; //Declare left force plate data array UINT16 rfpData[5]; //Declare right force plate data array //Initialize offset and temp variables for each force/moment from each belt INT32 lFyOffset = 0; INT32 lFzOffset = 0; INT32 lMxOffset = 0; INT32 lFxOffset = 0; INT32 lMyOffset = 0; UINT32 lFyTemp = 0; UINT32 lFzTemp = 0; UINT32 lMxTemp = 0; UINT32 lFxTemp = 0; UINT32 lMyTemp = 0; INT32 rFyOffset = 0; INT32 rFzOffset = 0; INT32 rMxOffset = 0; INT32 rFxOffset = 0; INT32 rMyOffset = 0; UINT32 rFyTemp = 0; UINT32 rFzTemp = 0; UINT32 rMxTemp = 0; UINT32 rFxTemp = 0; UINT32 rMyTemp = 0; UINT16 i = 0; //Initializes counter //Check for debug mode and set LEDs into debug mode configuration if (meanCheck == 0 && caliCheck == 0 && stumbleCheck == 0){ LED1 = LEDON; LED2 = LEDON; LED3 = LEDON; LED4 = LEDON; LED5 = LEDOFF; } //Initializes force plate data arrays to 0 for (i = 0; i < nData+2; i++){ lfpData[i] = 0; rfpData[i] = 0; } //If a trigger signal is needed //UINT16 triggerData = 0; //Reads trigger pin and converts from bits to voltage //triggerData = ADCRead(12); //Trigger = ((UINT32)Voltage * (UINT32)triggerData) / (UINT32)ADCBits; //Reads from ADC and records data in force plate arrays for (i = 0; i < nData; i++){ lfpData[i] = ADCRead(i); rfpData[i] = ADCRead(i+3); } for (i = 0; i < nData - 1; i++){ lfpData[i+3] = ADCRead(i+8); rfpData[i+3] = ADCRead(i+10); } //Converts from bits to voltage lFyTemp = ((UINT32)Voltage * (UINT32)lfpData[0]) / (UINT32)ADCBits; lFzTemp = ((UINT32)Voltage * (UINT32)lfpData[1]) / (UINT32)ADCBits; lMxTemp = ((UINT32)Voltage * (UINT32)lfpData[2]) / (UINT32)ADCBits; lFxTemp = ((UINT32)Voltage * (UINT32)lfpData[3]) / (UINT32)ADCBits; lMyTemp = ((UINT32)Voltage * (UINT32)lfpData[4]) / (UINT32)ADCBits; rFyTemp = ((UINT32)Voltage * (UINT32)rfpData[0]) / (UINT32)ADCBits; rFzTemp = ((UINT32)Voltage * (UINT32)rfpData[1]) / (UINT32)ADCBits; rMxTemp = ((UINT32)Voltage * (UINT32)rfpData[2]) / (UINT32)ADCBits; rFxTemp = ((UINT32)Voltage * (UINT32)rfpData[3]) / (UINT32)ADCBits; rMyTemp = ((UINT32)Voltage * (UINT32)rfpData[4]) / (UINT32)ADCBits; //Offsets from MeanZero function are converted from forces/moments back to voltage lFxOffset = (lFxMean) / FxGain; lFyOffset = (lFyMean) / FyGain; lFzOffset = (lFzMean) / FzGain; lMxOffset = (lMxMean) / MxGain; lMyOffset = (lMyMean) / MyGain; rFxOffset = (rFxMean) / FxGain; rFyOffset = (rFyMean) / FyGain; rFzOffset = (rFzMean) / FzGain; rMxOffset = (rMxMean) / MxGain; rMyOffset = (rMyMean) / MyGain; //Converts from voltage to force/moment with offset removed lFy = (INT32)FyGain * (lFyTemp - lFyOffset); lFz = (INT32)FzGain * (lFzTemp - lFzOffset); lMx = (INT32)1000 * (INT32)MxGain * (lMxTemp - lMxOffset); lFx = (INT32)FxGain * (lFxTemp - lFxOffset); lMy = (INT32)1000 * (INT32)MyGain * (lMyTemp - lMyOffset); rFy = (INT32)FyGain * (rFyTemp - rFyOffset); rFz = (INT32)FzGain * (rFzTemp - rFzOffset); rMx = (INT32)1000 * (INT32)MxGain * (rMxTemp - rMxOffset); rFx = (INT32)FxGain * (rFxTemp - rFxOffset); rMy = (INT32)1000 * (INT32)MyGain * (rMyTemp - rMyOffset); //Initializes COP variables lyCOP = 0; lxCOP = 0; ryCOP = 0; rxCOP = 0; /*Calculates COP if Fz is above a threshold (First checks for calibration * mode as threshold varies between stumble algorithm and calibration)*/ if (caliCheck == 0){ if ((lFz) > FzYThreshold) lyCOP = (h * lFy + lMx) / lFz; if ((rFz) > FzYThreshold) ryCOP = (h * rFy + rMx) / rFz; } if (caliCheck == 1){ if ((lFz) > CaliFzYThreshold) lyCOP = (h * lFy + lMx) / lFz; if ((rFz) > CaliFzYThreshold) ryCOP = (h * rFy + rMx) / rFz; } if ((lFz) > FzXThreshold) lxCOP = -(h * lFx - lMy) / lFz; if ((rFz) > FzXThreshold) rxCOP = (h * rFx - rMy) / rFz; //Collects data from desired belt switch (LR){ case 1: //Left Belt Fy = lFy; Fz = lFz; Mx = lMx; yCOP[0] = lyCOP; break; case 2: //Right Belt Fy = rFy; Fz = rFz; Mx = rMx; yCOP[0] = ryCOP; break; default: break; } datime++; //Increments data acquisition timer } void BlockRelease(void) { //This function contains the Predictive Targeting Algorithm //Initializes a number of intermediate variables UINT16 i = 0; UINT32 strideSum = 0; UINT32 swingSum = 0; UINT32 lengthSum = 0; INT32 hsCOPSum = 0; INT32 toCOPSum = 0; UINT32 tripTime = 0; UINT32 stumbleTime = 0; UINT32 blockTime = 0; UINT32 tmblockTime = 0; UINT32 blockLength = 0; UINT32 hundotmLength = 0; UINT32 hundotoCOP = 0; UINT32 scalarGain = 0; UINT32 crossoverCheck = 0; UINT32 stutterCheck = 0; //Sets LEDs in Stumble Algorithm configuration LED1 = LEDON; LED2 = LEDON; LED3 = LEDON; LED4 = LEDON; LED5 = LEDON; /*Determines which calibration variables (entry position and time) to use * based on chosen treadmill belt (right/left)*/ switch (LR){ case 1: //Left Belt tmLength = ltmLength; latencyTime = llatencyTime; break; case 2: //Right Belt tmLength = rtmLength; latencyTime = rlatencyTime; break; default: break; } //Eliminates spikes in COP signal if ((time - COPTime) > hsThreshold){ //Detects heel strike if (yCOP[0] != 0 && yCOP[1] == 0){ hsIndex++; //Increments heel strike index /*Waits for certain number of strides before beginning to calculate * gait events*/ if (hsIndex > strideThreshold){ strideTime[0] = time - hsTime; //Calculates stride time //Checks if gait event arrays have been fully populated if (strideIndex == 9){ /*Checks for anamolous strides since the system expects users to be walking at steady *state, therefore short, choppy strides or elongated (or crossover) strides are ignored*/ crossoverCheck = ((UINT32)125 * (UINT32)strideMean) / (UINT32)100; stutterCheck = ((UINT32)75 * (UINT32)strideMean) / (UINT32)100; //If either check is true, system ignores previous stride if (strideTime[0] >= crossoverCheck && strideTime[0] <= stutterCheck){ strideFlag = 1; strideTime[0] = 0; } else strideFlag = 0; //Otherwise continues } } hsTime = time; //Gets heel strike time } } /*Checks if a certain number of samples have passed since heel strike was * detected*/ if (time == hsTime + 9){ //Checks for previous heel strike if (yCOP[9] != 0 && yCOP[10] == 0){ for (i = 0; i < (nCOP - 1); i++) hsCOPSum += yCOP[i]; //Sums past 10 COP values hsCOPMean = hsCOPSum / (nCOP - 1); //Averages past 10 COP values hsCOP = hsCOPMean; //Sets heel strike position equal to average /*Waits a designated number of strides (and ensures no checks have * failed) and calculates various gait metrics*/ if (hsIndex > strideThreshold && strideFlag == 0){ swingTime[0] = time - toTime; //Calculates swing time //make time hstime?? strideLength[0] = hsCOP - toCOP; //Calculates stride length //Sums gait metric arrays for (i = 0; i < nStride; i++){ strideSum += strideTime[i]; swingSum += swingTime[i]; lengthSum += strideLength[i]; } //Averages gait metric arrays strideMean = strideSum / (strideIndex + 1); swingMean = swingSum / (strideIndex + 1); lengthMean = lengthSum / (strideIndex + 1); strideIndex++; //Increments stride index (maximum value = array length) stridecountIndex++; //Increments stride count index (no maximum) if (strideIndex > 9) strideIndex = 9; //Enforces maximum //Shifts gait metric arrays for next value for(i = (nStride - 1); i > 0; i--){ strideTime[i] = strideTime[i - 1]; swingTime[i] = swingTime[i - 1]; strideLength[i] = strideLength[i - 1]; } } } } //Detects toe off if (yCOP[0] == 0 && yCOP[1] != 0){ //Eliminates spikes in COP signal if (time - hsTime > toThreshold && time - toTime > toThreshold){ for (i = 1; i < nCOP; i++) toCOPSum += yCOP[i]; //Sums past 10 COP values toCOPMean = toCOPSum / (nCOP - 1); //Averages past 10 COP values toCOP = toCOPMean; //Sets toe off position to average toIndex++; //Increments toe off index toTime = time; //Gets toe off time /*Calculates experimentally validated gain based on stride length * to correct for swing time measurement error*/ if (lengthMean != 0) scalarGain = (UINT32)60000 / (UINT32)lengthMean; //Calculates release time at designated stride (see Predicitve Targeting Algorithm section) if (stridecountIndex == (nStrides - strideOffset)){ tripTime = ((UINT32)scalarGain * (UINT32)130 * ((UINT32)swingPercent * (UINT32)swingMean)) / (UINT32)1000000; //Time the subjects foot travels from toe off to perturbation point stumbleTime = (swingPercent * lengthMean) / tmVelocity; //Time the block would travel from toe off point to perturbation point hundotmLength = (UINT32)100 * (UINT32)tmLength; hundotoCOP = (UINT32)100 * (UINT32)toCOP; blockLength = (hundotmLength - hundotoCOP); tmblockTime = (blockLength / tmVelocity); //Time the block travels from entering treadmill to toe off point blockTime = tmblockTime + latencyTime - stumbleTime; //Total block travel time releaseTime = time + strideBuffer * strideMean + tripTime - blockTime; //Calculates total release time //Checks to see that the release time is in the future not the past if (releaseTime < time){ releaseTime = releaseInitial; releaseFlag = 1; } } //Calculates release time again if negative delay if (stridecountIndex == nStrides - (strideOffset - 1) && releaseFlag == 1){ tripTime = ((UINT32)scalarGain * (UINT32)130 * ((UINT32)swingPercent * (UINT32)swingMean)) / (UINT32)1000000; //Time the subjects foot travels from toe off to perturbation point stumbleTime = (swingPercent * lengthMean) / tmVelocity; //Time the block would travel from toe off point to perturbation point hundotmLength = (UINT32)100 * (UINT32)tmLength; hundotoCOP = (UINT32)100 * (UINT32)toCOP; blockLength = (hundotmLength - hundotoCOP); tmblockTime = (blockLength / tmVelocity); //Time the block travels from entering treadmill to toe off point blockTime = tmblockTime + latencyTime - stumbleTime; //Total block travel time releaseTime = time + (strideBuffer + 1) * strideMean + tripTime - blockTime; //Calculates total release time with additional stride time added } } } //Checks for which belt switch (LR){ case 1: DO1 = (time >= releaseTime); //Turns on digital output to release magnet after release time DO1 = DO1 - (time > releaseTime + releaseOffset); //Turns off digital output to turn magnet back on after a certain amount of time break; case 2: DO2 = (time >= releaseTime); //Turns on digital output to release magnet after release time DO2 = DO2 - (time > releaseTime + releaseOffset); //Turns off digital output to turn magnet back on after a certain amount of time break; default: break; } if (stridecountIndex == nStrides - (strideOffset + 1)) releaseCheck = 1; //Release time debug variable if (time > releaseTime + 3000) releaseCheck = 0; //Resets debug variable if (yCOP[0] > 0) COPTime = time; for (i = (nCOP - 1); i > 0; i--) yCOP[i] = yCOP[i - 1]; //Shifts COP array time++; //Increments time } void SimulinkOutput(void) { //This function sends data to Simulink UINT16 i = 0; //Initializes counter variable //Initializes output CAN data arrays for (i = 0; i < nCAN; i++){ gData[i] = 0; if (i > 0) gData2[i-1] = 0; gData3[i] = 0; } //Outputs data to Simulink via CAN gData[0] = (Fy / 1000) + 5000; //AP force (scaled and offset) gData[1] = (Fz / 1000) + 5000; //V force (scaled and offset) gData[2] = (Mx / 1000000) + 5000; //ML moment (scaled and offset) gData[3] = yCOP[0]; //AP COP gData2[0] = Trigger; //Trigger signal; gData2[1] = time; //System time gData2[2] = releaseCheck; //Release debug variable gData3[0] = lyCOP; //Left AP COP gData3[1] = lxCOP; //Left ML COP gData3[2] = ryCOP; //Right AP COP gData3[3] = rxCOP; //Right ML COP //Outputs data via CAN to Simulink CanSend0x20(); CanSend0x21(); CanSend0x22(); } void MeanZero(void) { //This function performs the software zero for the force-instrumented treadmill //Sets LEDs to Zeroing configuration (indicates Zeroing in process) if (zeroTime < meanTime){ LED1 = LEDON; LED2 = LEDON; LED3 = LEDOFF; LED4 = LEDOFF; LED5 = LEDOFF; } UINT16 lfpData[5]; //Declare left force plate data array UINT16 rfpData[5]; //Declare right force plate data array //Initialize temp variables for each force/moment from each belt UINT32 lFyTemp = 0; UINT32 lFzTemp = 0; UINT32 lMxTemp = 0; UINT32 lFxTemp = 0; UINT32 lMyTemp = 0; UINT32 rFyTemp = 0; UINT32 rFzTemp = 0; UINT32 rMxTemp = 0; UINT32 rFxTemp = 0; UINT32 rMyTemp = 0; UINT16 i = 0; //Initializes counter //Initializes force plate data arrays to 0 for (i = 0; i < nData + 2; i++){ lfpData[i] = 0; rfpData[i] = 0; } //Reads from ADC and records data in force plate arrays for (i = 0; i < nData; i++){ lfpData[i] = ADCRead(i); rfpData[i] = ADCRead(i+3); } for (i = 0; i < nData - 1; i++){ lfpData[i+3] = ADCRead(i+8); rfpData[i+3] = ADCRead(i+10); } //Converts from bits to voltage lFyTemp = ((UINT32)Voltage * (UINT32)lfpData[0]) / (UINT32)ADCBits; lFzTemp = ((UINT32)Voltage * (UINT32)lfpData[1]) / (UINT32)ADCBits; lMxTemp = ((UINT32)Voltage * (UINT32)lfpData[2]) / (UINT32)ADCBits; lFxTemp = ((UINT32)Voltage * (UINT32)lfpData[3]) / (UINT32)ADCBits; lMyTemp = ((UINT32)Voltage * (UINT32)lfpData[4]) / (UINT32)ADCBits; rFyTemp = ((UINT32)Voltage * (UINT32)rfpData[0]) / (UINT32)ADCBits; rFzTemp = ((UINT32)Voltage * (UINT32)rfpData[1]) / (UINT32)ADCBits; rMxTemp = ((UINT32)Voltage * (UINT32)rfpData[2]) / (UINT32)ADCBits; rFxTemp = ((UINT32)Voltage * (UINT32)rfpData[3]) / (UINT32)ADCBits; rMyTemp = ((UINT32)Voltage * (UINT32)rfpData[4]) / (UINT32)ADCBits; //Converts voltage to force lFy = FyGain*lFyTemp; lFz = FzGain*lFzTemp; lMx = MxGain*lMxTemp; lFx = FxGain*lFxTemp; lMy = MyGain*lMyTemp; rFy = FyGain*rFyTemp; rFz = FzGain*rFzTemp; rMx = MxGain*rMxTemp; rFx = FxGain*rFxTemp; rMy = MyGain*rMyTemp; //Sums force offset values if (zeroTime < meanTime){ lFySum += lFy; lFzSum += lFz; lMxSum += lMx; lFxSum += lFx; lMySum += lMy; rFySum += rFy; rFzSum += rFz; rMxSum += rMx; rFxSum += rFx; rMySum += rMy; } zeroTime++; //Increments zeroing timer //Calculates mean offsets if (zeroTime >= meanTime){ lFyMean = lFySum / meanTime; lFzMean = lFzSum / meanTime; lMxMean = lMxSum / meanTime; lFxMean = lFxSum / meanTime; lMyMean = lMySum / meanTime; rFyMean = rFySum / meanTime; rFzMean = rFzSum / meanTime; rMxMean = rMxSum / meanTime; rFxMean = rFxSum / meanTime; rMyMean = rMySum / meanTime; //Sets LEDs in completed configuration LED1 = LEDON; LED2 = LEDOFF; LED3 = LEDON; LED4 = LEDOFF; LED5 = LEDON; } } void Calibration(void) { /*This function is used to calibrate the system to the position of each ramp by determining the entry point * on to the treadmill and the travel time down the ramp for each block*/ UINT16 i = 0; //Initializes counter //Checks calibration flag to run calibration if (caliCheck == 1){ //Configures LEDs in calibration configuration if (icali < 5){ LED1 = LEDON; LED2 = LEDON; LED3 = LEDON; LED4 = LEDOFF; LED5 = LEDOFF; } caltime++; //Increment calibration timer //If timer is below threshold electromagnets remain on if (caltime < 10000){ if (LR == 1)DO1 = 0; if (LR == 2)DO2 = 0; Trigger = 0; } //If timer is above threshold 1 and below threshold 2 release block else if (caltime > 10000 && caltime < 20000){ if (LR == 1)DO1 = 1; if (LR == 2)DO2 = 1; Trigger = 1; } //If timer is above threshold 2 turn electromagnets back on else if (caltime > 20000){ caltime = 0; //Resets calibration timer Trigger = 0; } if (pastTrigger == 0 && Trigger == 1) commandTime = datime; //Gets block release time if (yCOP[0] > 1500 && yCOP[1] == 0) COPCount = 0; //Sets COP counter to 0 when block reaches treadmill //Checks if less than 100 COP samples have been recorded since block reached treadmill if (COPCount < 101){ yCOPSum += (UINT64)yCOP[0]; //Sums COP samples //Checks if Fz is greater then stored force value (peak) and less than threshold of samples has passed if (Fz >= FzStore && COPCount < 51){ FzStore = Fz; //Sets current force reading as past FzTime = datime; //Records current time } COPCount++; //Increments COP counter } //Checks if 100 samples of COP have been recorded if (COPCount == 100){ blockCOP = (UINT64)yCOPSum / (UINT64)100; //Averages COP values for block entry position rampTime = FzTime - commandTime; //Calculates travel time of block on ramp tmLengthSum += blockCOP; //Adds entry position to overall sum latencyTimeSum += rampTime; //Adds travel time to overall sum icali++; //Increments calibration counter COPCount++; //Increments COP counter yCOPSum = 0; //Sets COP sum to 0 FzStore = 0; //Sets stored Fz to 0 FzTime = 0; //Sets Fz peak time to 0 } //Checks for 5 calibration trials if (icali == 5){ calitmLength = tmLengthSum / 5; //Averages overall entry position sum calilatencyTime = latencyTimeSum / 5; //Averages overall travel time sum } //Assigns entry position and travel time values to desired belt switch (LR){ case 1: //Left Belt ltmLength = calitmLength; llatencyTime = calilatencyTime; break; case 2: //Right Belt rtmLength = calitmLength; rlatencyTime = calilatencyTime; break; default: break; } //Sets LEDs in completed configuration if (icali >= 5){ LED1 = LEDON; LED2 = LEDOFF; LED3 = LEDOFF; LED4 = LEDOFF; LED5 = LEDON; caltime = 0; //Sets calibration timer to 0 } pastTrigger = Trigger; //Sets past trigger to trigger //Shifts COP array for (i = (nCOP - 1); i > 0; i--) yCOP[i] = yCOP[i - 1]; } //Checks if calibration flag is set to load previous calibration else if (caliCheck == 2){ //Initializes flags UINT16 caliCANCheck = 1; UINT32 calizeroCheck = 0; //While CAN flag is high while (caliCANCheck == 1){ CanParse(); //Receives input data from Simulink via CAN //Receives entry position and travel time for both belts ltmLength = CANData3[0]; llatencyTime = CANData3[1]; rtmLength = CANData3[2]; rlatencyTime = CANData3[3]; calizeroCheck = ltmLength*llatencyTime*rtmLength*rlatencyTime; //Check to ensure no dropped packets if (calizeroCheck != 0) caliCANCheck = 0; //Continue if no dropped packets } //Sets LEDs in completed configuration LED1 = LEDON; LED2 = LEDOFF; LED3 = LEDOFF; LED4 = LEDOFF; LED5 = LEDON; } } void Restart(void) { //This function restarts and reinitializes the system //Reinitializes several functions InitOsc(); // Located in Init.c InitGlobs(); // Located in Init.c TimersInit(); // Located in Timers.c Ecan1Init(); // Located in ECAN1Config.c ADCInit(); // Initialize ADC PWMInit(); // Initialize PWM //Sets all relevant variables to zero strideFlag = 0; releaseCheck = 0; restartCheck = 0; restartCount = 0; runTime = 0; pastTime = 0; diffTime = 0; latencyTime = 0; tmLength = 0; Fy = 0; Fz = 0; Mx = 0; Trigger = 0; lFy = 0; lFz = 0; lMx = 0; lFx = 0; lMy = 0; rFy = 0; rFz = 0; rMx = 0; rFx = 0; rMy = 0; lyCOP = 0; lxCOP = 0; ryCOP = 0; rxCOP = 0; time = 0; releaseTime = releaseInitial; //Sets release time to large value datime = 0; releaseFlag = 0; hsIndex = 0; toIndex = 0; strideIndex = 0; stridecountIndex = 0; hsTime = 0; toTime = 0; COPTime = 0; strideMean = 0; swingMean = 0; lengthMean = 0; hsCOP = 0; toCOP = 0; hsCOPMean = 0; toCOPMean = 0; zeroTime = 0; caltime = 0; lFySum = 0; lFzSum = 0; lMxSum = 0; lFxSum = 0; lMySum = 0; rFySum = 0; rFzSum = 0; rMxSum = 0; rFxSum = 0; rMySum = 0; pastTrigger = 0; commandTime = 0; COPCount = 101; yCOPSum = 0; FzStore = 0; FzTime = 0; tmLengthSum = 0; latencyTimeSum = 0; icali = 0; calitmLength = 0; calilatencyTime = 0; blockCOP = 0; rampTime = 0; CANCheck = 1; meanCheck = 3; caliCheck = 3; stumbleCheck = 3; zeroCheck = 0; //Turns digital outputs off (keeps electromagnets on) DO1 = 0; DO2 = 0; //Sets LEDs in restart configuration LED1 = LEDON; LED2 = LEDOFF; LED3 = LEDOFF; LED4 = LEDOFF; LED5 = LEDOFF; UINT16 i; //Initializes counter //Sets various arrays to zero for (i = 0; i < nCOP; i++){ yCOP[i] = 0; } for (i = 0; i < nStride; i++){ strideTime[i] = 0; swingTime[i] = 0; strideLength[i] = 0; } swingPercent = 0; nStrides = 0; tmVelocity = 0; LR = 0; for (i = 0; i < nCAN; i++){ CANData[i] = 3; CANData2[i] = 0; if (i < 3) CANData3[i] = 3; } //Receives mean values from Simulink via CAN while flag is high while (CANCheck==1) { CanParse(); //Receives input data from Simulink via CAN meanCheck = CANData[0]; //Zero Flag caliCheck = CANData[1]; //Calibration Flag stumbleCheck = CANData[2]; //Stumble Algorithm Flag runTime = CANData[3]; //Simulink run time (clock) swingPercent = CANData2[0]; //Desired percent swing of perturbation nStrides = CANData2[1]; //Number of strides until perturbation tmVelocity = CANData2[2]; //Treadmill velocity LR = CANData2[3]; //Treadmill belt zeroCheck = swingPercent*nStrides*tmVelocity*LR; //Checks for dropped packets if (zeroCheck != 0) CANCheck = 0; //Continues if no dropped packets } }