Francesco Pistone
/
Viaro_SpandiConcime_V3_inizio
copia12092018
main.cpp
- Committer:
- viaromassimo
- Date:
- 2018-06-29
- Revision:
- 4:1643fea75703
- Parent:
- 3:1cd8d048cd4a
- Child:
- 5:72bdc69d610d
File content as of revision 4:1643fea75703:
#include "main.h" //*************************************************** void DC_prepare(){ // direction or brake preparation if (DC_brake==1){ SDmotorInA=1; SDmotorInB=1; }else{ if (DC_forward==1){ SDmotorInA=1; SDmotorInB=0; }else{ SDmotorInA=0; SDmotorInB=1; } } // fault reading if (SDmotorInA==1){ SD_faultA=1; }else{ SD_faultA=0; } if (SDmotorInB==1){ SD_faultB=1; }else{ SD_faultB=0; } } //*************************************************** void ResetData(){ ReadWheelArray.clear(); ReadMotorArray.clear(); ElapsedM=0; ElapsedW=0; TempReadM=0; TempReadW=0; WheelData=0; MotorData=0; motorPID.reset(); WantedMotorSpeed=0; ReadMotorSpeed=0; motorPID.setProcessValue(0); } //*************************************************** void OffTheMotor(){ MotorPwmPin.write(0); // duty cycle=off - off the motor DC_brake=1; DC_prepare(); SecurityStop=1; ResetData(); } //*************************************************** void ReadWheelInterrupt(){ //interrupt that read the wheel sensor TempReadW=uSWheel.read_us(); if(TempReadW>200){ //thi s filter is used to avoid double readings if(TempReadW>800000){ TempReadW=800000; } ReadWheelArray.push(TempReadW); uSWheel.reset(); WheelData=1; } } //*************************************************** void ReadMotorInterrupt(){ //interrupt that read the motor sensor TempReadM=uSMotor.read_us(); if(TempReadM>200){ //thi s filter is used to avoid double readings, since the code takes 10 to 40 uS to cycle if(TempReadM>100000){ TempReadM=100000; } ReadMotorArray.push(TempReadM); uSMotor.reset(); MotorData=1; } } //*************************************************** void CheckPercentSwitch(){ //this below checks the add/take off Percent switches, with the control of the vibrations applied if(PlusPercentPin==1){ if(ChangeStatusOfPlusPercentPinToHigh==1){ ChangeStatusOfPlusPercentPinToHigh=0; vibPlusToHSwitch.start(); }else if(vibPlusToHSwitch.read_ms()>VibrationTimer){ vibPlusToHSwitch.stop(); vibPlusToLSwitch.stop(); vibPlusToLSwitch.reset(); ChangeStatusOfPlusPercentPinToLow=1; PercentPlus=1; } }else{ if(ChangeStatusOfPlusPercentPinToLow==1){ ChangeStatusOfPlusPercentPinToLow=0; vibPlusToLSwitch.start(); }else if(vibPlusToLSwitch.read_ms()>VibrationTimer){ vibPlusToLSwitch.stop(); vibPlusToHSwitch.stop(); vibPlusToHSwitch.reset(); ChangeStatusOfPlusPercentPinToHigh=1; PercentPlus=0; } } if(MinusPercentPin==1){ if(ChangeStatusOfMinusPercentPinToHigh==1){ ChangeStatusOfMinusPercentPinToHigh=0; vibMinusToHSwitch.start(); }else if(vibMinusToHSwitch.read_ms()>VibrationTimer){ vibMinusToHSwitch.stop(); vibMinusToLSwitch.stop(); vibMinusToLSwitch.reset(); ChangeStatusOfMinusPercentPinToLow=1; PercentMinus=1; } }else{ if(ChangeStatusOfMinusPercentPinToLow==1){ ChangeStatusOfMinusPercentPinToLow=0; vibMinusToLSwitch.start(); }else if(vibMinusToLSwitch.read_ms()>VibrationTimer){ vibMinusToLSwitch.stop(); vibMinusToHSwitch.stop(); vibMinusToHSwitch.reset(); ChangeStatusOfMinusPercentPinToHigh=1; PercentMinus=0; } } if(PercentPlus==0 && PercentMinus==1){ Percent=1.1f; } if(PercentPlus==1 && PercentMinus==0){ Percent=0.9f; } if(PercentPlus==1 && PercentMinus==1){ Percent=1.0f; } if(PercentPlus==0 && PercentMinus==0){ Percent=1.0f; //GENERATE AN ERROR SOMEHOW } } //*************************************************** void writelog(){ pc.printf("\nMotorSpeedCorrected %f",MotorSpeedCorrected); pc.printf(" - WantedMotorSpeed %d",WantedMotorSpeed); pc.printf(" - ReadMotorSpeed %d",ReadMotorSpeed); pc.printf(" - ElapsedW %d",ElapsedW); pc.printf(" - TempReadW %d",TempReadW); pc.printf(" - ElapsedM %d",ElapsedM); pc.printf(" - TempReadM %d",TempReadM); pc.printf(" - Plus %d",PercentPlus); pc.printf(" - Minus %d",PercentMinus); pc.printf(" - Security %d",SecurityStop); } //*************************************************** void CheckMotorCorrection(){ if(MotorSensorError==0 && SecurityStop==0){ CheckPercentSwitch(); WantedMotorSpeed=(float)((60000000/(ElapsedW*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent); // =(( WHEEL_REVOLUTIONS/MINUTES )* RATIO CHANGE * +/- % ); rev/min if(WantedMotorSpeed<MinMotorSpeed){ WantedMotorSpeed=0; } if(MachineType=="electric"){ motorPID.setSetPoint(WantedMotorSpeed); motorPID.setProcessValue(ReadMotorSpeed); MotorSpeedCorrected=motorPID.compute(); if(WantedMotorSpeed!=0 && uSMotor.read_us()>1000000){ MotorSensorSecurityCheck=MotorSensorSecurityCheck++; if(MotorSensorSecurityCheck>50){ MotorSensorError=1; // OFF and ON to reset } }else{ MotorSensorSecurityCheck=0; } } if(MachineType=="idraulic"){ MotorSpeedCorrected=(float)((WantedMotorSpeed*(100-MinMotorPwm)/MaxMotorSpeed)+MinMotorPwm)/100.0f; //this pwm calculation accept the variable "MinMotorPwm" to set the minimal pwm point at which the motor can start. the scale won't be 0-255, but MinMotorPwm-255 } MotorPwmPin.write(MotorSpeedCorrected); DC_brake=0; DC_prepare(); writelog(); }else{ OffTheMotor(); } } //*************************************************** void CheckTimeElapsedFromLastTrig(){ //check time Elapsed from last wheel trig if((uSWheel.read_us()>(ReadWheelArray.last()*DecelerationControlRatio)) ){ //if the lag between the last read and now is more than *DecelerationControlRatio* times of the last trig or more of a max limit, or equal 1 -so, no reads- (this is an error control), it assumes the speed is too slow to act and resets OffTheMotor(); }else{ SecurityStop=0; } } //*************************************************** void SetMotorSpeed(){ CheckTimeElapsedFromLastTrig(); if(WheelData==1){ ElapsedW=0; a=0; for(i=0; i<(ReadWheelArray.size()-1); i++){ if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){ //avoid reading of 1 or 0 values in the array a++; //number of all the valid readings ElapsedW=ElapsedW+ReadWheelArray[i]; //all the valid readings are summed } } ElapsedW=ElapsedW/a; //calculation of avarage of the valid readings WheelData=0; } if(MotorData==1){ ElapsedM=0; a=0; for(i=0; i<(ReadMotorArray.size()-1); i++){ if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){ //avoid reading of 1 or 0 values in the array a++; //number of all the valid readings ElapsedM=ElapsedM+ReadMotorArray[i]; //all the valid readings are summed } } ElapsedM=ElapsedM/a; MotorData=0; } if(ElapsedM==0){ ReadMotorSpeed=0; }else{ ReadMotorSpeed=float(60000000/(ElapsedM*4.0f)); // rev/min } } //*************************************************** void SwitchOnOff(){ //this below checks the on/off Percent switches, with the control of the vibrations and the Previous status (for variable reset purposes) applied // pc.printf("\nTIME %d ",uSCycle.read_us()); // uSCycle.reset(); if(OnOffPin==0){ if(ChangeStatusOfOnOffPinToHigh==1){ ChangeStatusOfOnOffPinToHigh=0; vibOnOffSwitch.start(); }else if(vibOnOffSwitch.read_ms()>VibrationTimer){ vibOnOffSwitch.stop(); if(ChangeFromOffToOn==1){ ChangeFromOffToOn=0; } DC_brake=0; DC_prepare(); SetMotorSpeed(); } }else{ ChangeStatusOfOnOffPinToHigh=1; vibOnOffSwitch.stop(); vibOnOffSwitch.reset(); ChangeFromOffToOn=1; MotorSensorError=0; OffTheMotor(); } } //*************************************************** //*************************************************** //*************************************************** int main(){ wait(1); uSMotor.start(); uSWheel.start(); MotorPwmPin.period_us(2000); // freq 1khz MotorPwmPin.write(0.0f); // duty cycle=off OffTheMotor(); MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds ReadWheelArray.clear(); ReadMotorArray.clear(); pc.printf("\n\n Hello World\n\n"); WheelSensorPin.rise(&ReadWheelInterrupt); //interrupt for wheel sensor, trigs when changes from 0 to 1 MotorSensorPin.rise(&ReadMotorInterrupt); //interrupt for motor sensor, trigs when changes from 0 to 1 motorPID.setInputLimits(0, 10000); //70*secondosinaminute*trigsperwheelrevolutions motorPID.setOutputLimits(0.0f, 1.0f); motorPID.setMode(1); motorPID.setSetPoint(1000); //*************************************************** //*************************************************** //*************************************************** while(1){ SwitchOnOff(); } }