Francesco Pistone
/
Viaro_SpandiConcime_V3_inizio
copia12092018
main.cpp
- Committer:
- root@developer-sjc-indigo-compiler.local.mbed.org
- Date:
- 2018-11-16
- Revision:
- 13:4d6114864f2d
- Parent:
- 9:6fb26643eecd
File content as of revision 13:4d6114864f2d:
#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(){ } //*************************************************** 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){ //francesco 17/09/2018 //WantedMotorSpeed=0; WantedMotorSpeed = MinMotorSpeed; } if(WantedMotorSpeed > MaxMotorSpeed){ WantedMotorSpeed = MaxMotorSpeed; } if(MachineType=="electric"){ motorPID.setSetPoint(WantedMotorSpeed); motorPID.setProcessValue(ReadMotorSpeed); MotorSpeedCorrected=motorPID.compute(); if(WantedMotorSpeed!=0 && uSMotor.read_us()>1000000){ //francesco 17/09/2018 -->> test 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); //francesco 17/09/2018 -> test //MotorPwmPin.write(0.2f); //MotorPwmPin.write(0.9f); DC_brake=0; DC_prepare(); #if defined(logActive) 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); #endif }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{ //francesco 14/09/2018 -->> il rapporto di riduzione è già stato considerato ReadMotorSpeed=float(60000000/ElapsedM); // rev/min //ReadMotorSpeed=float(60000000/(ElapsedM*4.0f)); // rev/min if(ReadMotorSpeed < MinMotorSpeed){ ReadMotorSpeed = MinMotorSpeed; } if(ReadMotorSpeed > MaxMotorSpeed){ ReadMotorSpeed = MaxMotorSpeed; } } } //*************************************************** 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(); //francesco 14/09/2018 -->> alzo il limite //motorPID.setInputLimits(0, 1000000); -->> il test non ha buoni frutti, rimetto l'originale 10000 motorPID.setInputLimits(1000, 10000); //70*secondosinaminute*trigsperwheelrevolutions motorPID.setOutputLimits(0.0f, 1.0f); motorPID.setMode(1); //MotorPwmPin.period_us(2000); // freq 1khz //francesco 13/09/2018 -->> periodo sbagliato se si vuole una frequenza di un khz MotorPwmPin.period_us(1000); MotorPwmPin.write(0.0f); // duty cycle=off OffTheMotor(); MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds ReadWheelArray.clear(); ReadMotorArray.clear(); #if defined(logActive) pc.printf("\n\n Salute, Capo!\n\n"); #endif 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 //francesco 14/09/2018 -->> non capisco perchè fornire un set point fisso //motorPID.setSetPoint(1000); //*************************************************** //*************************************************** //*************************************************** while(1){ SwitchOnOff(); } }