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();
  }
}