Francesco Pistone
/
Viaro_SpandiConcime_V3_inizio
copia12092018
main.cpp
- Committer:
- viaromassimo
- Date:
- 2018-06-15
- Revision:
- 2:5111c6c0639e
- Parent:
- 1:59c30e854dfb
- Child:
- 3:1cd8d048cd4a
File content as of revision 2:5111c6c0639e:
#include "main.h" //*************************************************** void DC_prepare(){ // direction or brake preparation if (DC_brake==1){ SDmotorInA=0; SDmotorInB=0; }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 OffTheMotor(){ MotorPwmPin.write(0); // duty cycle=off - off the motor StopTheMotorWeGoTooSlow=1; DC_brake=1; DC_prepare(); } //*************************************************** void ReadWheelInterrupt(){ //interrupt that read the wheel sensor TempRead=uSWheel.read_us(); pc.printf("\nruota"); ReadWheelArray.push(TempRead); uSWheel.reset(); } //*************************************************** void ReadMotorInterrupt(){ //interrupt that read the motor sensor TempReadM=uSMotor.read_us(); ReadMotorArray.push(TempReadM); uSMotor.reset(); } //*************************************************** 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; PlusPercentPinTimerToHigh=millis(); }else if((millis()-PlusPercentPinTimerToHigh)>VibrationTimer){ ChangeStatusOfPlusPercentPinToLow=1; PercentPlus=1; } }else{ if(ChangeStatusOfPlusPercentPinToLow==1){ ChangeStatusOfPlusPercentPinToLow=0; PlusPercentPinTimerToLow=millis(); }else if((millis()-PlusPercentPinTimerToLow)>VibrationTimer){ ChangeStatusOfPlusPercentPinToHigh=1; PercentPlus=0; } } if(MinusPercentPin==1){ if(ChangeStatusOfMinusPercentPinToHigh==1){ ChangeStatusOfMinusPercentPinToHigh=0; MinusPercentPinTimerToHigh=millis(); }else if((millis()-MinusPercentPinTimerToHigh)>VibrationTimer){ ChangeStatusOfMinusPercentPinToLow=1; PercentMinus=1; } }else{ if(ChangeStatusOfMinusPercentPinToLow==1){ ChangeStatusOfMinusPercentPinToLow=0; MinusPercentPinTimerToLow=millis(); }else if((millis()-MinusPercentPinTimerToLow)>VibrationTimer){ ChangeStatusOfMinusPercentPinToHigh=1; PercentMinus=0; } } if(PercentPlus==1 && PercentMinus==0){ Percent=(float)1.1; } if(PercentPlus==0 && PercentMinus==1){ Percent=(float)0.9; } if(PercentPlus==0 && PercentMinus==0){ Percent=(float)1.0; } if(PercentPlus==1 && PercentMinus==1){ Percent=(float)1.0; //GENERATE AN ERROR SOMEHOW } } //*************************************************** void CheckMotorCorrection(){ pc.printf("\nMotorSensorPin.read %d ", MotorSensorPin.read()); 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; if((StopTheMotorWeGoTooSlow==0) && (ElapsedM!=1) && uSMotor.read_us()>(2*ElapsedM)){ MotorSensorSecurityCheck=MotorSensorSecurityCheck++; if(MotorSensorSecurityCheck>50){ MotorSensorError=1; } }else{ if(ReadMotorArray.last()==1){ MotorSpeedCorrected=10000; }else{ ReadMotorSpeed=float(60000000/(ElapsedM*4)); // rev/min motorPID.setSetPoint(WantedMotorSpeed); motorPID.setProcessValue(ReadMotorSpeed); MotorSpeedCorrected=motorPID.compute(); pc.printf("\n\nMotorSpeedCorrectedF %f ",MotorSpeedCorrected); pc.printf("\nWantedMotorSpeed %d ",WantedMotorSpeed); pc.printf("\nReadMotorSpeed %d ",ReadMotorSpeed); pc.printf("\nElapsed %d ",Elapsed); pc.printf("\nElapsedM4 %d ",Elapsed*4); pc.printf("\nTrigsPerWheelRevolution %d ",TrigsPerWheelRevolution); pc.printf("\nWheelToMotorRatio %f ",WheelToMotorRatio); pc.printf("\nPercent %f ",Percent); pc.printf("\nMotorSpeedPwm %f ",MotorSpeedPwm); //MotorCorrection=WantedMotorSpeed/ReadMotorSpeed; MotorSensorSecurityCheck=0; } } } //*************************************************** void CheckTimeElapsedFromLastTrig(){ //check time Elapsed from last wheel trig // pc.printf("\nuSWheel.read_us %d ",uSWheel.read_us()); // pc.printf("\nReadWheelArray.last %d ",ReadWheelArray.last()); if((uSWheel.read_us()>(ReadWheelArray.last()*DecelerationControlRatio)) || (ReadWheelArray.last()==1) ){ //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{ if(StopTheMotorWeGoTooSlow==1){ uSMotor.reset(); //reset the the millis of PreviousM read for (i=0; i<ConstArrayIns; i++) { ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1" MotorCorrection=1.0f; } } StopTheMotorWeGoTooSlow=0; } } //*************************************************** void SetMotorSpeed(){ led=0; CheckTimeElapsedFromLastTrig(); if(MotorSensorError==1){ OffTheMotor(); // GENERATE MOTOR SENSOR ERROR } if(StopTheMotorWeGoTooSlow==0){ Elapsed=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 Elapsed=Elapsed+ReadWheelArray[i]; //all the valid readings are summed } } Elapsed=Elapsed/a; //calculation of avarage of the valid readings CheckPercentSwitch(); // BELOW=(( WHEEL_REVOLUTIONS/MINUTES )* RATIO CHANGE * +/- % ); WantedMotorSpeed=(float)((60000000/(Elapsed*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent); // rev/min 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); led=1; } } //*************************************************** 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 if(OnOffPin==1){ if(ChangeStatusOfOnOffPinToHigh==1){ ChangeStatusOfOnOffPinToHigh=0; OnOffPinTimerToHigh=millis(); }else if((millis()-OnOffPinTimerToHigh)>VibrationTimer){ if(ChangeFromOffToOn==1){ uSWheel.reset(); uSMotor.reset(); MotorCorrection=(float)1; MotorSensorError=0; MotorSensorSecurityCheck=0; StopTheMotorWeGoTooSlow=0; for (i=0; i<ConstArrayIns; i++) { // | ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1" ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1" } // | ChangeFromOffToOn=0; } DC_brake=0; DC_prepare(); SetMotorSpeed(); } }else{ ChangeStatusOfOnOffPinToHigh=1; for (i=0; i<ConstArrayIns; i++) { ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1" ReadMotorArray.push((int)1); // | this fills the motor sensor array with "1" } ChangeFromOffToOn=1; OffTheMotor(); } } //*************************************************** //*************************************************** //*************************************************** int main() { wait(1); millisStart(); uSMotor.start(); uSWheel.start(); WheelToMotorRatio=(float)(1800/18); //RUOTA FONICA > MOTORE MotorPwmPin.period_us(2000); // freq 1khz MotorPwmPin.write(0.0f); // duty cycle=off OffTheMotor(); if(MachineType=="electric"){ MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds } for (i=0; i<ConstArrayIns; i++) { ReadWheelArray.push((int)1); ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1" } pc.printf("Hello World"); pc.printf("%d",millis()); 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 //Analog input from 0.0 to 3.3V motorPID.setInputLimits(0, 42000); //70*secondosinaminute*trigsperwheelrevolutions //Pwm output from 0.0 to 1.0 motorPID.setOutputLimits(0.0f, 1.0f); motorPID.setMode(1); //If there's a bias. //motorPID.setBias(0.3); //motorPID.setMode(AUTO); //We want the process variable to be 1.7V motorPID.setSetPoint(21000); //*************************************************** //*************************************************** //*************************************************** while (1){ SwitchOnOff(); } }