Diff: main.cpp

Revision:

0:b0a79a3a9da8

Child:

1:59c30e854dfb

diff -r 000000000000 -r b0a79a3a9da8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 13 08:41:23 2018 +0000
@@ -0,0 +1,268 @@
+#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();
+  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(){
+  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
+  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)(180/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();
+  }
+}
\ No newline at end of file