Francesco Pistone
copia12092018
Diff: main.cpp
- Revision:
- 0:b0a79a3a9da8
- Child:
- 1:59c30e854dfb
--- /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