main.cpp

00001 #include "main.h"
00002 
00003 //***************************************************
00004 void DC_prepare(){
00005     // direction or brake preparation
00006     if (DC_brake==1){
00007         SDmotorInA=1;
00008         SDmotorInB=1;
00009     }else{
00010         if (DC_forward==1){
00011             SDmotorInA=1;
00012             SDmotorInB=0;
00013         }else{
00014             SDmotorInA=0;
00015             SDmotorInB=1;
00016         }
00017     }
00018     // fault reading
00019     if (SDmotorInA==1){
00020         SD_faultA=1;
00021     }else{
00022         SD_faultA=0;
00023     }
00024     if (SDmotorInB==1){
00025         SD_faultB=1;
00026     }else{
00027         SD_faultB=0;
00028     }
00029 }
00030 
00031 //***************************************************
00032 void ResetData(){
00033     ReadWheelArray.clear();
00034     ReadMotorArray.clear();
00035     ElapsedM=0;
00036     ElapsedW=0;
00037     TempReadM=0;
00038     TempReadW=0;
00039     WheelData=0;
00040     MotorData=0;
00041     motorPID.reset();
00042     WantedMotorSpeed=0;
00043     ReadMotorSpeed=0;
00044     motorPID.setProcessValue(0);
00045 }
00046 
00047 //***************************************************
00048 void OffTheMotor(){
00049     MotorPwmPin.write(0);  // duty cycle=off - off the motor
00050     DC_brake=1;
00051     DC_prepare();
00052     SecurityStop=1;
00053     ResetData();
00054 }
00055 
00056 //***************************************************
00057 void ReadWheelInterrupt(){  //interrupt that read the wheel sensor
00058     TempReadW=uSWheel.read_us();
00059     if(TempReadW>200){  //thi s filter is used to avoid double readings
00060         if(TempReadW>800000){
00061             TempReadW=800000;
00062         }
00063     ReadWheelArray.push(TempReadW);
00064     uSWheel.reset();
00065     WheelData=1;
00066     }
00067 }
00068 
00069 //***************************************************
00070 void ReadMotorInterrupt(){  //interrupt that read the motor sensor
00071     TempReadM=uSMotor.read_us();
00072     if(TempReadM>200){  //thi s filter is used to avoid double readings, since the code takes 10 to 40 uS to cycle
00073         if(TempReadM>100000){
00074             TempReadM=100000;
00075         }
00076     ReadMotorArray.push(TempReadM);
00077     uSMotor.reset();
00078     MotorData=1;
00079     }
00080 }
00081 
00082 //***************************************************
00083 void CheckPercentSwitch(){  //this below checks the add/take off Percent switches, with the control of the vibrations applied
00084     if(PlusPercentPin==1){
00085         if(ChangeStatusOfPlusPercentPinToHigh==1){
00086             ChangeStatusOfPlusPercentPinToHigh=0;
00087         vibPlusToHSwitch.start();
00088         }else if(vibPlusToHSwitch.read_ms()>VibrationTimer){
00089             vibPlusToHSwitch.stop();
00090             vibPlusToLSwitch.stop();
00091             vibPlusToLSwitch.reset();
00092             ChangeStatusOfPlusPercentPinToLow=1;
00093             PercentPlus=1;
00094         }
00095     }else{
00096         if(ChangeStatusOfPlusPercentPinToLow==1){
00097             ChangeStatusOfPlusPercentPinToLow=0;
00098             vibPlusToLSwitch.start();
00099         }else if(vibPlusToLSwitch.read_ms()>VibrationTimer){
00100             vibPlusToLSwitch.stop();
00101             vibPlusToHSwitch.stop();
00102             vibPlusToHSwitch.reset();
00103             ChangeStatusOfPlusPercentPinToHigh=1;
00104             PercentPlus=0;
00105         }
00106     }
00107 
00108     if(MinusPercentPin==1){
00109         if(ChangeStatusOfMinusPercentPinToHigh==1){
00110             ChangeStatusOfMinusPercentPinToHigh=0;
00111             vibMinusToHSwitch.start();
00112         }else if(vibMinusToHSwitch.read_ms()>VibrationTimer){
00113             vibMinusToHSwitch.stop();
00114             vibMinusToLSwitch.stop();
00115             vibMinusToLSwitch.reset();
00116             ChangeStatusOfMinusPercentPinToLow=1;
00117             PercentMinus=1;
00118         }
00119     }else{
00120         if(ChangeStatusOfMinusPercentPinToLow==1){
00121             ChangeStatusOfMinusPercentPinToLow=0;
00122             vibMinusToLSwitch.start();
00123         }else if(vibMinusToLSwitch.read_ms()>VibrationTimer){
00124             vibMinusToLSwitch.stop();
00125             vibMinusToHSwitch.stop();
00126             vibMinusToHSwitch.reset();
00127             ChangeStatusOfMinusPercentPinToHigh=1;
00128             PercentMinus=0;
00129         }
00130     }
00131 
00132     if(PercentPlus==0 && PercentMinus==1){
00133         Percent=1.1f;
00134     }
00135     if(PercentPlus==1 && PercentMinus==0){
00136         Percent=0.9f;
00137     }
00138     if(PercentPlus==1 && PercentMinus==1){
00139         Percent=1.0f;
00140     }
00141     if(PercentPlus==0 && PercentMinus==0){
00142         Percent=1.0f;
00143     //GENERATE AN ERROR SOMEHOW
00144     }
00145 }
00146 
00147 //***************************************************
00148 void writelog(){
00149 }
00150 
00151 //***************************************************
00152 void CheckMotorCorrection(){
00153     if(MotorSensorError==0 && SecurityStop==0){
00154         CheckPercentSwitch();
00155         WantedMotorSpeed=(float)((60000000/(ElapsedW*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent);  // =((       WHEEL_REVOLUTIONS/MINUTES        )*   RATIO CHANGE  * +/- % ); rev/min
00156         if(WantedMotorSpeed<MinMotorSpeed){
00157             WantedMotorSpeed=0;
00158         }
00159         
00160         if(MachineType=="electric"){
00161             motorPID.setSetPoint(WantedMotorSpeed);
00162             motorPID.setProcessValue(ReadMotorSpeed);
00163             MotorSpeedCorrected=motorPID.compute();
00164             if(WantedMotorSpeed!=0 && uSMotor.read_us()>1000000){
00165                 MotorSensorSecurityCheck=MotorSensorSecurityCheck++;
00166                 if(MotorSensorSecurityCheck>50){
00167                     MotorSensorError=1;  // OFF and ON to reset
00168                 }
00169             }else{
00170                 MotorSensorSecurityCheck=0;
00171             }
00172         }
00173         
00174         if(MachineType=="idraulic"){
00175             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
00176         }
00177         
00178         MotorPwmPin.write(MotorSpeedCorrected);
00179         DC_brake=0;
00180         DC_prepare();
00181 #if defined(logActive)
00182         pc.printf("\nMotorSpeedCorrected %f",MotorSpeedCorrected);
00183         pc.printf(" - WantedMotorSpeed %d",WantedMotorSpeed);
00184         pc.printf(" - ReadMotorSpeed %d",ReadMotorSpeed);
00185         pc.printf(" - ElapsedW %d",ElapsedW);
00186         pc.printf(" - TempReadW %d",TempReadW);
00187         pc.printf(" - ElapsedM %d",ElapsedM);
00188         pc.printf(" - TempReadM %d",TempReadM);
00189         pc.printf(" - Plus %d",PercentPlus);
00190         pc.printf(" - Minus %d",PercentMinus);
00191         pc.printf(" - Security %d",SecurityStop);
00192 #endif
00193     }else{
00194         OffTheMotor();
00195     }
00196 }
00197 
00198 //***************************************************
00199 void CheckTimeElapsedFromLastTrig(){  //check time Elapsed from last wheel trig
00200     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
00201         OffTheMotor();
00202     }else{
00203         SecurityStop=0;
00204     }
00205 }
00206 
00207 //***************************************************
00208 void SetMotorSpeed(){
00209     CheckTimeElapsedFromLastTrig();
00210 
00211     if(WheelData==1){
00212         ElapsedW=0;
00213         a=0;
00214         for(i=0; i<(ReadWheelArray.size()-1); i++){
00215             if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){  //avoid reading of 1 or 0 values in the array
00216             a++;  //number of all the valid readings
00217             ElapsedW=ElapsedW+ReadWheelArray[i];  //all the valid readings are summed
00218             }
00219         }
00220         ElapsedW=ElapsedW/a;  //calculation of avarage of the valid readings
00221         WheelData=0;
00222     }
00223 
00224     if(MotorData==1){
00225         ElapsedM=0;
00226         a=0;
00227         for(i=0; i<(ReadMotorArray.size()-1); i++){
00228             if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){  //avoid reading of 1 or 0 values in the array
00229                 a++;  //number of all the valid readings
00230                 ElapsedM=ElapsedM+ReadMotorArray[i];  //all the valid readings are summed
00231             }
00232          }
00233          ElapsedM=ElapsedM/a;
00234          MotorData=0;
00235     }
00236 
00237     if(ElapsedM==0){
00238         ReadMotorSpeed=0;
00239     }else{
00240         ReadMotorSpeed=float(60000000/(ElapsedM*4.0f));  // rev/min
00241     }
00242 }
00243 
00244 //***************************************************
00245 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
00246 //  pc.printf("\nTIME %d ",uSCycle.read_us());
00247 //  uSCycle.reset();
00248     if(OnOffPin==0){
00249         if(ChangeStatusOfOnOffPinToHigh==1){
00250             ChangeStatusOfOnOffPinToHigh=0;
00251             vibOnOffSwitch.start();
00252         }else if(vibOnOffSwitch.read_ms()>VibrationTimer){
00253             vibOnOffSwitch.stop();
00254             if(ChangeFromOffToOn==1){
00255                 ChangeFromOffToOn=0;
00256             }
00257             DC_brake=0;
00258             DC_prepare();
00259             SetMotorSpeed();
00260         }
00261     }else{
00262         ChangeStatusOfOnOffPinToHigh=1;
00263         vibOnOffSwitch.stop();
00264         vibOnOffSwitch.reset();
00265         ChangeFromOffToOn=1;
00266         MotorSensorError=0;
00267         OffTheMotor();
00268     }
00269 }
00270 
00271 //***************************************************
00272 //***************************************************
00273 //***************************************************
00274 int main(){
00275     wait(1);
00276     uSMotor.start();
00277     uSWheel.start();
00278     MotorPwmPin.period_us(2000);  // freq 1khz
00279     MotorPwmPin.write(0.0f);  // duty cycle=off
00280     OffTheMotor();
00281     MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1);  //check motor correction every xx seconds
00282     ReadWheelArray.clear();
00283     ReadMotorArray.clear();
00284 #if defined(logActive)
00285     pc.printf("\n\n                         Salute, Capo!\n\n");
00286 #endif
00287     WheelSensorPin.rise(&ReadWheelInterrupt);  //interrupt for wheel sensor, trigs when changes from 0 to 1
00288     MotorSensorPin.rise(&ReadMotorInterrupt);  //interrupt for motor sensor, trigs when changes from 0 to 1
00289 
00290     motorPID.setInputLimits(0, 10000); //70*secondosinaminute*trigsperwheelrevolutions
00291     motorPID.setOutputLimits(0.0f, 1.0f);
00292     motorPID.setMode(1);
00293     motorPID.setSetPoint(1000);
00294 
00295 //***************************************************
00296 //***************************************************
00297 //***************************************************
00298     while(1){
00299         SwitchOnOff();
00300     }
00301 }