File content as of revision 5:72bdc69d610d:

#include "main.h"

//***************************************************
void DC_prepare(){
    // direction or brake preparation
    if (DC_brake==1){
        SDmotorInA=1;
        SDmotorInB=1;
    }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 ResetData(){
    ReadWheelArray.clear();
    ReadMotorArray.clear();
    ElapsedM=0;
    ElapsedW=0;
    TempReadM=0;
    TempReadW=0;
    WheelData=0;
    MotorData=0;
    motorPID.reset();
    WantedMotorSpeed=0;
    ReadMotorSpeed=0;
    motorPID.setProcessValue(0);
}

//***************************************************
void OffTheMotor(){
    MotorPwmPin.write(0);  // duty cycle=off - off the motor
    DC_brake=1;
    DC_prepare();
    SecurityStop=1;
    ResetData();
}

//***************************************************
void ReadWheelInterrupt(){  //interrupt that read the wheel sensor
    TempReadW=uSWheel.read_us();
    if(TempReadW>200){  //thi s filter is used to avoid double readings
        if(TempReadW>800000){
            TempReadW=800000;
        }
    ReadWheelArray.push(TempReadW);
    uSWheel.reset();
    WheelData=1;
    }
}

//***************************************************
void ReadMotorInterrupt(){  //interrupt that read the motor sensor
    TempReadM=uSMotor.read_us();
    if(TempReadM>200){  //thi s filter is used to avoid double readings, since the code takes 10 to 40 uS to cycle
        if(TempReadM>100000){
            TempReadM=100000;
        }
    ReadMotorArray.push(TempReadM);
    uSMotor.reset();
    MotorData=1;
    }
}

//***************************************************
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;
        vibPlusToHSwitch.start();
        }else if(vibPlusToHSwitch.read_ms()>VibrationTimer){
            vibPlusToHSwitch.stop();
            vibPlusToLSwitch.stop();
            vibPlusToLSwitch.reset();
            ChangeStatusOfPlusPercentPinToLow=1;
            PercentPlus=1;
        }
    }else{
        if(ChangeStatusOfPlusPercentPinToLow==1){
            ChangeStatusOfPlusPercentPinToLow=0;
            vibPlusToLSwitch.start();
        }else if(vibPlusToLSwitch.read_ms()>VibrationTimer){
            vibPlusToLSwitch.stop();
            vibPlusToHSwitch.stop();
            vibPlusToHSwitch.reset();
            ChangeStatusOfPlusPercentPinToHigh=1;
            PercentPlus=0;
        }
    }

    if(MinusPercentPin==1){
        if(ChangeStatusOfMinusPercentPinToHigh==1){
            ChangeStatusOfMinusPercentPinToHigh=0;
            vibMinusToHSwitch.start();
        }else if(vibMinusToHSwitch.read_ms()>VibrationTimer){
            vibMinusToHSwitch.stop();
            vibMinusToLSwitch.stop();
            vibMinusToLSwitch.reset();
            ChangeStatusOfMinusPercentPinToLow=1;
            PercentMinus=1;
        }
    }else{
        if(ChangeStatusOfMinusPercentPinToLow==1){
            ChangeStatusOfMinusPercentPinToLow=0;
            vibMinusToLSwitch.start();
        }else if(vibMinusToLSwitch.read_ms()>VibrationTimer){
            vibMinusToLSwitch.stop();
            vibMinusToHSwitch.stop();
            vibMinusToHSwitch.reset();
            ChangeStatusOfMinusPercentPinToHigh=1;
            PercentMinus=0;
        }
    }

    if(PercentPlus==0 && PercentMinus==1){
        Percent=1.1f;
    }
    if(PercentPlus==1 && PercentMinus==0){
        Percent=0.9f;
    }
    if(PercentPlus==1 && PercentMinus==1){
        Percent=1.0f;
    }
    if(PercentPlus==0 && PercentMinus==0){
        Percent=1.0f;
    //GENERATE AN ERROR SOMEHOW
    }
}

//***************************************************
void writelog(){
}

//***************************************************
void CheckMotorCorrection(){
    if(MotorSensorError==0 && SecurityStop==0){
        CheckPercentSwitch();
        WantedMotorSpeed=(float)((60000000/(ElapsedW*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent);  // =((       WHEEL_REVOLUTIONS/MINUTES        )*   RATIO CHANGE  * +/- % ); rev/min
        if(WantedMotorSpeed<MinMotorSpeed){
            WantedMotorSpeed=0;
        }
        
        if(MachineType=="electric"){
            motorPID.setSetPoint(WantedMotorSpeed);
            motorPID.setProcessValue(ReadMotorSpeed);
            MotorSpeedCorrected=motorPID.compute();
            if(WantedMotorSpeed!=0 && uSMotor.read_us()>1000000){
                MotorSensorSecurityCheck=MotorSensorSecurityCheck++;
                if(MotorSensorSecurityCheck>50){
                    MotorSensorError=1;  // OFF and ON to reset
                }
            }else{
                MotorSensorSecurityCheck=0;
            }
        }
        
        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);
        DC_brake=0;
        DC_prepare();
#if defined(logActive)
        pc.printf("\nMotorSpeedCorrected %f",MotorSpeedCorrected);
        pc.printf(" - WantedMotorSpeed %d",WantedMotorSpeed);
        pc.printf(" - ReadMotorSpeed %d",ReadMotorSpeed);
        pc.printf(" - ElapsedW %d",ElapsedW);
        pc.printf(" - TempReadW %d",TempReadW);
        pc.printf(" - ElapsedM %d",ElapsedM);
        pc.printf(" - TempReadM %d",TempReadM);
        pc.printf(" - Plus %d",PercentPlus);
        pc.printf(" - Minus %d",PercentMinus);
        pc.printf(" - Security %d",SecurityStop);
#endif
    }else{
        OffTheMotor();
    }
}

//***************************************************
void CheckTimeElapsedFromLastTrig(){  //check time Elapsed from last wheel trig
    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
        OffTheMotor();
    }else{
        SecurityStop=0;
    }
}

//***************************************************
void SetMotorSpeed(){
    CheckTimeElapsedFromLastTrig();

    if(WheelData==1){
        ElapsedW=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
            ElapsedW=ElapsedW+ReadWheelArray[i];  //all the valid readings are summed
            }
        }
        ElapsedW=ElapsedW/a;  //calculation of avarage of the valid readings
        WheelData=0;
    }

    if(MotorData==1){
        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;
         MotorData=0;
    }

    if(ElapsedM==0){
        ReadMotorSpeed=0;
    }else{
        ReadMotorSpeed=float(60000000/(ElapsedM*4.0f));  // rev/min
    }
}

//***************************************************
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
//  pc.printf("\nTIME %d ",uSCycle.read_us());
//  uSCycle.reset();
    if(OnOffPin==0){
        if(ChangeStatusOfOnOffPinToHigh==1){
            ChangeStatusOfOnOffPinToHigh=0;
            vibOnOffSwitch.start();
        }else if(vibOnOffSwitch.read_ms()>VibrationTimer){
            vibOnOffSwitch.stop();
            if(ChangeFromOffToOn==1){
                ChangeFromOffToOn=0;
            }
            DC_brake=0;
            DC_prepare();
            SetMotorSpeed();
        }
    }else{
        ChangeStatusOfOnOffPinToHigh=1;
        vibOnOffSwitch.stop();
        vibOnOffSwitch.reset();
        ChangeFromOffToOn=1;
        MotorSensorError=0;
        OffTheMotor();
    }
}

//***************************************************
//***************************************************
//***************************************************
int main(){
    wait(1);
    uSMotor.start();
    uSWheel.start();
    MotorPwmPin.period_us(2000);  // freq 1khz
    MotorPwmPin.write(0.0f);  // duty cycle=off
    OffTheMotor();
    MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1);  //check motor correction every xx seconds
    ReadWheelArray.clear();
    ReadMotorArray.clear();
#if defined(logActive)
    pc.printf("\n\n                         Salute, Capo!\n\n");
#endif
    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

    motorPID.setInputLimits(0, 10000); //70*secondosinaminute*trigsperwheelrevolutions
    motorPID.setOutputLimits(0.0f, 1.0f);
    motorPID.setMode(1);
    motorPID.setSetPoint(1000);

//***************************************************
//***************************************************
//***************************************************
    while(1){
        SwitchOnOff();
    }
}