main.cpp

00001 #include "main.h"
00002 
00003 //***************************************************
00004 void DC_prepare(){
00005     // direction or brake preparation
00006     if (DC_brake==1){
00007         SDmotorInA=0;
00008         SDmotorInB=0;
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 OffTheMotor(){
00033   MotorPwmPin.write(0);  // duty cycle=off - off the motor
00034   StopTheMotorWeGoTooSlow=1;
00035   DC_brake=1;
00036   DC_prepare();
00037 }
00038 
00039 //***************************************************
00040 void ReadWheelInterrupt(){  //interrupt that read the wheel sensor
00041   TempRead=uSWheel.read_us();
00042       pc.printf("\nruota");
00043   ReadWheelArray.push(TempRead);
00044   uSWheel.reset();
00045 }
00046 
00047 //***************************************************
00048 void ReadMotorInterrupt(){  //interrupt that read the motor sensor
00049   TempReadM=uSMotor.read_us();
00050   ReadMotorArray.push(TempReadM);
00051   uSMotor.reset();
00052 }
00053 
00054 //***************************************************
00055 void CheckPercentSwitch(){  //this below checks the add/take off Percent switches, with the control of the vibrations applied
00056   if(PlusPercentPin==1){
00057     if(ChangeStatusOfPlusPercentPinToHigh==1){
00058       ChangeStatusOfPlusPercentPinToHigh=0;
00059       PlusPercentPinTimerToHigh=millis();
00060     }else if((millis()-PlusPercentPinTimerToHigh)>VibrationTimer){
00061       ChangeStatusOfPlusPercentPinToLow=1;
00062       PercentPlus=1;
00063     }
00064   }else{
00065     if(ChangeStatusOfPlusPercentPinToLow==1){
00066       ChangeStatusOfPlusPercentPinToLow=0;
00067       PlusPercentPinTimerToLow=millis();
00068     }else if((millis()-PlusPercentPinTimerToLow)>VibrationTimer){
00069       ChangeStatusOfPlusPercentPinToHigh=1;
00070       PercentPlus=0;
00071     }
00072   }
00073 
00074   if(MinusPercentPin==1){
00075     if(ChangeStatusOfMinusPercentPinToHigh==1){
00076       ChangeStatusOfMinusPercentPinToHigh=0;
00077       MinusPercentPinTimerToHigh=millis();
00078     }else if((millis()-MinusPercentPinTimerToHigh)>VibrationTimer){
00079       ChangeStatusOfMinusPercentPinToLow=1;
00080       PercentMinus=1;
00081     }
00082   }else{
00083     if(ChangeStatusOfMinusPercentPinToLow==1){
00084       ChangeStatusOfMinusPercentPinToLow=0;
00085       MinusPercentPinTimerToLow=millis();
00086     }else if((millis()-MinusPercentPinTimerToLow)>VibrationTimer){
00087       ChangeStatusOfMinusPercentPinToHigh=1;
00088       PercentMinus=0;
00089     }
00090   }
00091 
00092   if(PercentPlus==1 && PercentMinus==0){
00093     Percent=(float)1.1;
00094   }
00095   if(PercentPlus==0 && PercentMinus==1){
00096     Percent=(float)0.9;
00097   }
00098   if(PercentPlus==0 && PercentMinus==0){
00099     Percent=(float)1.0;
00100   }
00101   if(PercentPlus==1 && PercentMinus==1){
00102     Percent=(float)1.0;
00103     //GENERATE AN ERROR SOMEHOW
00104   }
00105 }
00106 
00107 //***************************************************
00108 void CheckMotorCorrection(){
00109       pc.printf("\nMotorSensorPin.read %d  ", MotorSensorPin.read());
00110   ElapsedM=0;
00111   a=0;
00112   for(i=0; i<(ReadMotorArray.size()-1); i++){
00113     if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){  //avoid reading of 1 or 0 values in the array
00114       a++;  //number of all the valid readings
00115       ElapsedM=ElapsedM+ReadMotorArray[i];  //all the valid readings are summed
00116     }
00117   }
00118   ElapsedM=ElapsedM/a;
00119   if((StopTheMotorWeGoTooSlow==0) && (ElapsedM!=1) && uSMotor.read_us()>(2*ElapsedM)){
00120   MotorSensorSecurityCheck=MotorSensorSecurityCheck++;
00121     if(MotorSensorSecurityCheck>50){
00122       MotorSensorError=1;
00123     }
00124   }else{
00125     if(ReadMotorArray.last()==1){
00126       MotorSpeedCorrected=10000;
00127     }else{
00128       ReadMotorSpeed=float(60000000/(ElapsedM*4));  // rev/min
00129       motorPID.setSetPoint(WantedMotorSpeed);
00130       motorPID.setProcessValue(ReadMotorSpeed);
00131       MotorSpeedCorrected=motorPID.compute();
00132       pc.printf("\n\nMotorSpeedCorrectedF %f ",MotorSpeedCorrected);
00133       pc.printf("\nWantedMotorSpeed %d ",WantedMotorSpeed);
00134       pc.printf("\nReadMotorSpeed %d ",ReadMotorSpeed);
00135       pc.printf("\nElapsed %d ",Elapsed);
00136       pc.printf("\nElapsedM4 %d ",Elapsed*4);
00137       pc.printf("\nTrigsPerWheelRevolution %d ",TrigsPerWheelRevolution);
00138       pc.printf("\nWheelToMotorRatio %f ",WheelToMotorRatio);
00139       pc.printf("\nPercent %f ",Percent);
00140       pc.printf("\nMotorSpeedPwm %f ",MotorSpeedPwm);
00141       //MotorCorrection=WantedMotorSpeed/ReadMotorSpeed;
00142       MotorSensorSecurityCheck=0;
00143     }
00144   }
00145 }
00146 
00147 //***************************************************
00148 void CheckTimeElapsedFromLastTrig(){  //check time Elapsed from last wheel trig
00149 //      pc.printf("\nuSWheel.read_us %d ",uSWheel.read_us());
00150 //      pc.printf("\nReadWheelArray.last %d ",ReadWheelArray.last());
00151   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
00152     OffTheMotor();
00153   }else{
00154     if(StopTheMotorWeGoTooSlow==1){
00155       uSMotor.reset();  //reset the the millis of PreviousM read
00156       for (i=0; i<ConstArrayIns; i++) {
00157         ReadMotorArray.push((int)1);  //                          | this fills the wheel sensor array with "1"
00158         MotorCorrection=1.0f;
00159       }
00160     }
00161     StopTheMotorWeGoTooSlow=0;
00162   }
00163 }
00164 
00165 //***************************************************
00166 void SetMotorSpeed(){
00167   led=0;
00168   CheckTimeElapsedFromLastTrig();
00169   if(MotorSensorError==1){
00170     OffTheMotor();
00171     // GENERATE MOTOR SENSOR ERROR
00172   }
00173   if(StopTheMotorWeGoTooSlow==0){
00174     Elapsed=0;
00175     a=0;
00176     for(i=0; i<(ReadWheelArray.size()-1); i++){
00177       if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){  //avoid reading of 1 or 0 values in the array
00178         a++;  //number of all the valid readings
00179         Elapsed=Elapsed+ReadWheelArray[i];  //all the valid readings are summed
00180       }
00181     }
00182     Elapsed=Elapsed/a;  //calculation of avarage of the valid readings
00183     CheckPercentSwitch();
00184     //   BELOW=((       WHEEL_REVOLUTIONS/MINUTES        )*   RATIO CHANGE  * +/- % );
00185     WantedMotorSpeed=(float)((60000000/(Elapsed*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent);  // rev/min
00186     if(MachineType=="idraulic"){
00187           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
00188     }    
00189     MotorPwmPin.write(MotorSpeedCorrected);
00190     led=1;
00191   }   
00192 }
00193 
00194 //***************************************************
00195 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
00196   if(OnOffPin==1){
00197     if(ChangeStatusOfOnOffPinToHigh==1){
00198       ChangeStatusOfOnOffPinToHigh=0;
00199       OnOffPinTimerToHigh=millis();
00200     }else if((millis()-OnOffPinTimerToHigh)>VibrationTimer){
00201       if(ChangeFromOffToOn==1){
00202         uSWheel.reset();
00203         uSMotor.reset();
00204         MotorCorrection=(float)1;
00205         MotorSensorError=0;
00206         MotorSensorSecurityCheck=0;
00207         StopTheMotorWeGoTooSlow=0;
00208         for (i=0; i<ConstArrayIns; i++) {  //                       |
00209           ReadWheelArray.push((int)1);  //                          | this fills the wheel sensor array with "1"
00210           ReadMotorArray.push((int)1);  //                          | this fills the wheel sensor array with "1"
00211         }  //                                                       |
00212         ChangeFromOffToOn=0;
00213       }
00214       DC_brake=0;
00215       DC_prepare();
00216       SetMotorSpeed();
00217     }
00218   }else{
00219       ChangeStatusOfOnOffPinToHigh=1;
00220         for (i=0; i<ConstArrayIns; i++) {
00221           ReadWheelArray.push((int)1);  //                          | this fills the wheel sensor array with "1"
00222           ReadMotorArray.push((int)1);  //                          | this fills the motor sensor array with "1"
00223         }
00224         ChangeFromOffToOn=1;
00225         OffTheMotor();
00226   }
00227 }
00228 
00229 //***************************************************
00230 //***************************************************
00231 //***************************************************
00232 int main() {
00233   wait(1);
00234   millisStart();
00235   uSMotor.start();
00236   uSWheel.start();
00237   WheelToMotorRatio=(float)(1800/18); //RUOTA FONICA > MOTORE
00238   MotorPwmPin.period_us(2000);  // freq 1khz
00239   MotorPwmPin.write(0.0f);  // duty cycle=off
00240   OffTheMotor();
00241   if(MachineType=="electric"){
00242     MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1);  //check motor correction every xx seconds
00243   }
00244   for (i=0; i<ConstArrayIns; i++) {
00245     ReadWheelArray.push((int)1);
00246     ReadMotorArray.push((int)1);  //                          | this fills the wheel sensor array with "1"
00247   }
00248     pc.printf("Hello World");
00249     pc.printf("%d",millis());
00250 
00251   WheelSensorPin.rise(&ReadWheelInterrupt);  //interrupt for wheel sensor, trigs when changes from 0 to 1
00252   MotorSensorPin.rise(&ReadMotorInterrupt);  //interrupt for motor sensor, trigs when changes from 0 to 1
00253 
00254  
00255   //Analog input from 0.0 to 3.3V
00256   motorPID.setInputLimits(0, 42000); //70*secondosinaminute*trigsperwheelrevolutions
00257   //Pwm output from 0.0 to 1.0
00258   motorPID.setOutputLimits(0.0f, 1.0f);
00259   motorPID.setMode(1);
00260   //If there's a bias.
00261   //motorPID.setBias(0.3);
00262   //motorPID.setMode(AUTO);
00263   //We want the process variable to be 1.7V
00264   motorPID.setSetPoint(21000);
00265 
00266 //***************************************************
00267 //***************************************************
00268 //***************************************************
00269   while (1){
00270     SwitchOnOff();
00271   }
00272 }