Francesco Pistone / Mbed 2 deprecated Viaro_SpandiConcime_V3_inizio

copia12092018

Dependencies:   mbed

main.cpp@0:b0a79a3a9da8, 2018-06-13 (annotated)

Committer:
nerit
Date:
Wed Jun 13 08:41:23 2018 +0000
Revision:
0:b0a79a3a9da8
Child:
1:59c30e854dfb
ControlloSpandiconcime V2 Viaro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nerit 0:b0a79a3a9da8 1 #include "main.h"
nerit 0:b0a79a3a9da8 2
nerit 0:b0a79a3a9da8 3 //***************************************************
nerit 0:b0a79a3a9da8 4 void DC_prepare(){
nerit 0:b0a79a3a9da8 5 // direction or brake preparation
nerit 0:b0a79a3a9da8 6 if (DC_brake==1){
nerit 0:b0a79a3a9da8 7 SDmotorInA=0;
nerit 0:b0a79a3a9da8 8 SDmotorInB=0;
nerit 0:b0a79a3a9da8 9 }else{
nerit 0:b0a79a3a9da8 10 if (DC_forward==1){
nerit 0:b0a79a3a9da8 11 SDmotorInA=1;
nerit 0:b0a79a3a9da8 12 SDmotorInB=0;
nerit 0:b0a79a3a9da8 13 }else{
nerit 0:b0a79a3a9da8 14 SDmotorInA=0;
nerit 0:b0a79a3a9da8 15 SDmotorInB=1;
nerit 0:b0a79a3a9da8 16 }
nerit 0:b0a79a3a9da8 17 }
nerit 0:b0a79a3a9da8 18 // fault reading
nerit 0:b0a79a3a9da8 19 if (SDmotorInA==1){
nerit 0:b0a79a3a9da8 20 SD_faultA=1;
nerit 0:b0a79a3a9da8 21 }else{
nerit 0:b0a79a3a9da8 22 SD_faultA=0;
nerit 0:b0a79a3a9da8 23 }
nerit 0:b0a79a3a9da8 24 if (SDmotorInB==1){
nerit 0:b0a79a3a9da8 25 SD_faultB=1;
nerit 0:b0a79a3a9da8 26 }else{
nerit 0:b0a79a3a9da8 27 SD_faultB=0;
nerit 0:b0a79a3a9da8 28 }
nerit 0:b0a79a3a9da8 29 }
nerit 0:b0a79a3a9da8 30
nerit 0:b0a79a3a9da8 31 //***************************************************
nerit 0:b0a79a3a9da8 32 void OffTheMotor(){
nerit 0:b0a79a3a9da8 33 MotorPwmPin.write(0); // duty cycle=off - off the motor
nerit 0:b0a79a3a9da8 34 StopTheMotorWeGoTooSlow=1;
nerit 0:b0a79a3a9da8 35 DC_brake=1;
nerit 0:b0a79a3a9da8 36 DC_prepare();
nerit 0:b0a79a3a9da8 37 }
nerit 0:b0a79a3a9da8 38
nerit 0:b0a79a3a9da8 39 //***************************************************
nerit 0:b0a79a3a9da8 40 void ReadWheelInterrupt(){ //interrupt that read the wheel sensor
nerit 0:b0a79a3a9da8 41 TempRead=uSWheel.read_us();
nerit 0:b0a79a3a9da8 42 ReadWheelArray.push(TempRead);
nerit 0:b0a79a3a9da8 43 uSWheel.reset();
nerit 0:b0a79a3a9da8 44 }
nerit 0:b0a79a3a9da8 45
nerit 0:b0a79a3a9da8 46 //***************************************************
nerit 0:b0a79a3a9da8 47 void ReadMotorInterrupt(){ //interrupt that read the motor sensor
nerit 0:b0a79a3a9da8 48 TempReadM=uSMotor.read_us();
nerit 0:b0a79a3a9da8 49 ReadMotorArray.push(TempReadM);
nerit 0:b0a79a3a9da8 50 uSMotor.reset();
nerit 0:b0a79a3a9da8 51 }
nerit 0:b0a79a3a9da8 52
nerit 0:b0a79a3a9da8 53 //***************************************************
nerit 0:b0a79a3a9da8 54 void CheckPercentSwitch(){ //this below checks the add/take off Percent switches, with the control of the vibrations applied
nerit 0:b0a79a3a9da8 55 if(PlusPercentPin==1){
nerit 0:b0a79a3a9da8 56 if(ChangeStatusOfPlusPercentPinToHigh==1){
nerit 0:b0a79a3a9da8 57 ChangeStatusOfPlusPercentPinToHigh=0;
nerit 0:b0a79a3a9da8 58 PlusPercentPinTimerToHigh=millis();
nerit 0:b0a79a3a9da8 59 }else if((millis()-PlusPercentPinTimerToHigh)>VibrationTimer){
nerit 0:b0a79a3a9da8 60 ChangeStatusOfPlusPercentPinToLow=1;
nerit 0:b0a79a3a9da8 61 PercentPlus=1;
nerit 0:b0a79a3a9da8 62 }
nerit 0:b0a79a3a9da8 63 }else{
nerit 0:b0a79a3a9da8 64 if(ChangeStatusOfPlusPercentPinToLow==1){
nerit 0:b0a79a3a9da8 65 ChangeStatusOfPlusPercentPinToLow=0;
nerit 0:b0a79a3a9da8 66 PlusPercentPinTimerToLow=millis();
nerit 0:b0a79a3a9da8 67 }else if((millis()-PlusPercentPinTimerToLow)>VibrationTimer){
nerit 0:b0a79a3a9da8 68 ChangeStatusOfPlusPercentPinToHigh=1;
nerit 0:b0a79a3a9da8 69 PercentPlus=0;
nerit 0:b0a79a3a9da8 70 }
nerit 0:b0a79a3a9da8 71 }
nerit 0:b0a79a3a9da8 72
nerit 0:b0a79a3a9da8 73 if(MinusPercentPin==1){
nerit 0:b0a79a3a9da8 74 if(ChangeStatusOfMinusPercentPinToHigh==1){
nerit 0:b0a79a3a9da8 75 ChangeStatusOfMinusPercentPinToHigh=0;
nerit 0:b0a79a3a9da8 76 MinusPercentPinTimerToHigh=millis();
nerit 0:b0a79a3a9da8 77 }else if((millis()-MinusPercentPinTimerToHigh)>VibrationTimer){
nerit 0:b0a79a3a9da8 78 ChangeStatusOfMinusPercentPinToLow=1;
nerit 0:b0a79a3a9da8 79 PercentMinus=1;
nerit 0:b0a79a3a9da8 80 }
nerit 0:b0a79a3a9da8 81 }else{
nerit 0:b0a79a3a9da8 82 if(ChangeStatusOfMinusPercentPinToLow==1){
nerit 0:b0a79a3a9da8 83 ChangeStatusOfMinusPercentPinToLow=0;
nerit 0:b0a79a3a9da8 84 MinusPercentPinTimerToLow=millis();
nerit 0:b0a79a3a9da8 85 }else if((millis()-MinusPercentPinTimerToLow)>VibrationTimer){
nerit 0:b0a79a3a9da8 86 ChangeStatusOfMinusPercentPinToHigh=1;
nerit 0:b0a79a3a9da8 87 PercentMinus=0;
nerit 0:b0a79a3a9da8 88 }
nerit 0:b0a79a3a9da8 89 }
nerit 0:b0a79a3a9da8 90
nerit 0:b0a79a3a9da8 91 if(PercentPlus==1 && PercentMinus==0){
nerit 0:b0a79a3a9da8 92 Percent=(float)1.1;
nerit 0:b0a79a3a9da8 93 }
nerit 0:b0a79a3a9da8 94 if(PercentPlus==0 && PercentMinus==1){
nerit 0:b0a79a3a9da8 95 Percent=(float)0.9;
nerit 0:b0a79a3a9da8 96 }
nerit 0:b0a79a3a9da8 97 if(PercentPlus==0 && PercentMinus==0){
nerit 0:b0a79a3a9da8 98 Percent=(float)1.0;
nerit 0:b0a79a3a9da8 99 }
nerit 0:b0a79a3a9da8 100 if(PercentPlus==1 && PercentMinus==1){
nerit 0:b0a79a3a9da8 101 Percent=(float)1.0;
nerit 0:b0a79a3a9da8 102 //GENERATE AN ERROR SOMEHOW
nerit 0:b0a79a3a9da8 103 }
nerit 0:b0a79a3a9da8 104 }
nerit 0:b0a79a3a9da8 105
nerit 0:b0a79a3a9da8 106 //***************************************************
nerit 0:b0a79a3a9da8 107 void CheckMotorCorrection(){
nerit 0:b0a79a3a9da8 108 ElapsedM=0;
nerit 0:b0a79a3a9da8 109 a=0;
nerit 0:b0a79a3a9da8 110 for(i=0; i<(ReadMotorArray.size()-1); i++){
nerit 0:b0a79a3a9da8 111 if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){ //avoid reading of 1 or 0 values in the array
nerit 0:b0a79a3a9da8 112 a++; //number of all the valid readings
nerit 0:b0a79a3a9da8 113 ElapsedM=ElapsedM+ReadMotorArray[i]; //all the valid readings are summed
nerit 0:b0a79a3a9da8 114 }
nerit 0:b0a79a3a9da8 115 }
nerit 0:b0a79a3a9da8 116 ElapsedM=ElapsedM/a;
nerit 0:b0a79a3a9da8 117 if((StopTheMotorWeGoTooSlow==0) && (ElapsedM!=1) && uSMotor.read_us()>(2*ElapsedM)){
nerit 0:b0a79a3a9da8 118 MotorSensorSecurityCheck=MotorSensorSecurityCheck++;
nerit 0:b0a79a3a9da8 119 if(MotorSensorSecurityCheck>50){
nerit 0:b0a79a3a9da8 120 MotorSensorError=1;
nerit 0:b0a79a3a9da8 121 }
nerit 0:b0a79a3a9da8 122 }else{
nerit 0:b0a79a3a9da8 123 if(ReadMotorArray.last()==1){
nerit 0:b0a79a3a9da8 124 MotorSpeedCorrected=10000;
nerit 0:b0a79a3a9da8 125 }else{
nerit 0:b0a79a3a9da8 126 ReadMotorSpeed=float(60000000/(ElapsedM*4)); // rev/min
nerit 0:b0a79a3a9da8 127 motorPID.setSetPoint(WantedMotorSpeed);
nerit 0:b0a79a3a9da8 128 motorPID.setProcessValue(ReadMotorSpeed);
nerit 0:b0a79a3a9da8 129 MotorSpeedCorrected=motorPID.compute();
nerit 0:b0a79a3a9da8 130 // pc.printf("\n\nMotorSpeedCorrectedF %f ",MotorSpeedCorrected);
nerit 0:b0a79a3a9da8 131 // pc.printf("\nWantedMotorSpeed %d ",WantedMotorSpeed);
nerit 0:b0a79a3a9da8 132 // pc.printf("\nReadMotorSpeed %d ",ReadMotorSpeed);
nerit 0:b0a79a3a9da8 133 // pc.printf("\nElapsed %d ",Elapsed);
nerit 0:b0a79a3a9da8 134 // pc.printf("\nElapsedM4 %d ",Elapsed*4);
nerit 0:b0a79a3a9da8 135 // pc.printf("\nTrigsPerWheelRevolution %d ",TrigsPerWheelRevolution);
nerit 0:b0a79a3a9da8 136 // pc.printf("\nWheelToMotorRatio %f ",WheelToMotorRatio);
nerit 0:b0a79a3a9da8 137 // pc.printf("\nPercent %f ",Percent);
nerit 0:b0a79a3a9da8 138 // pc.printf("\nMotorSpeedPwm %f ",MotorSpeedPwm);
nerit 0:b0a79a3a9da8 139 //MotorCorrection=WantedMotorSpeed/ReadMotorSpeed;
nerit 0:b0a79a3a9da8 140 MotorSensorSecurityCheck=0;
nerit 0:b0a79a3a9da8 141 }
nerit 0:b0a79a3a9da8 142 }
nerit 0:b0a79a3a9da8 143 }
nerit 0:b0a79a3a9da8 144
nerit 0:b0a79a3a9da8 145 //***************************************************
nerit 0:b0a79a3a9da8 146 void CheckTimeElapsedFromLastTrig(){ //check time Elapsed from last wheel trig
nerit 0:b0a79a3a9da8 147 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
nerit 0:b0a79a3a9da8 148 OffTheMotor();
nerit 0:b0a79a3a9da8 149 }else{
nerit 0:b0a79a3a9da8 150 if(StopTheMotorWeGoTooSlow==1){
nerit 0:b0a79a3a9da8 151 uSMotor.reset(); //reset the the millis of PreviousM read
nerit 0:b0a79a3a9da8 152 for (i=0; i<ConstArrayIns; i++) {
nerit 0:b0a79a3a9da8 153 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 154 MotorCorrection=1.0f;
nerit 0:b0a79a3a9da8 155 }
nerit 0:b0a79a3a9da8 156 }
nerit 0:b0a79a3a9da8 157 StopTheMotorWeGoTooSlow=0;
nerit 0:b0a79a3a9da8 158 }
nerit 0:b0a79a3a9da8 159 }
nerit 0:b0a79a3a9da8 160
nerit 0:b0a79a3a9da8 161 //***************************************************
nerit 0:b0a79a3a9da8 162 void SetMotorSpeed(){
nerit 0:b0a79a3a9da8 163 led=0;
nerit 0:b0a79a3a9da8 164 CheckTimeElapsedFromLastTrig();
nerit 0:b0a79a3a9da8 165 if(MotorSensorError==1){
nerit 0:b0a79a3a9da8 166 OffTheMotor();
nerit 0:b0a79a3a9da8 167 // GENERATE MOTOR SENSOR ERROR
nerit 0:b0a79a3a9da8 168 }
nerit 0:b0a79a3a9da8 169 if(StopTheMotorWeGoTooSlow==0){
nerit 0:b0a79a3a9da8 170 Elapsed=0;
nerit 0:b0a79a3a9da8 171 a=0;
nerit 0:b0a79a3a9da8 172 for(i=0; i<(ReadWheelArray.size()-1); i++){
nerit 0:b0a79a3a9da8 173 if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){ //avoid reading of 1 or 0 values in the array
nerit 0:b0a79a3a9da8 174 a++; //number of all the valid readings
nerit 0:b0a79a3a9da8 175 Elapsed=Elapsed+ReadWheelArray[i]; //all the valid readings are summed
nerit 0:b0a79a3a9da8 176 }
nerit 0:b0a79a3a9da8 177 }
nerit 0:b0a79a3a9da8 178 Elapsed=Elapsed/a; //calculation of avarage of the valid readings
nerit 0:b0a79a3a9da8 179 CheckPercentSwitch();
nerit 0:b0a79a3a9da8 180 // BELOW=(( WHEEL_REVOLUTIONS/MINUTES )* RATIO CHANGE * +/- % );
nerit 0:b0a79a3a9da8 181 WantedMotorSpeed=(float)((60000000/(Elapsed*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent); // rev/min
nerit 0:b0a79a3a9da8 182 if(MachineType=="idraulic"){
nerit 0:b0a79a3a9da8 183 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
nerit 0:b0a79a3a9da8 184 }
nerit 0:b0a79a3a9da8 185 MotorPwmPin.write(MotorSpeedCorrected);
nerit 0:b0a79a3a9da8 186 led=1;
nerit 0:b0a79a3a9da8 187 }
nerit 0:b0a79a3a9da8 188 }
nerit 0:b0a79a3a9da8 189
nerit 0:b0a79a3a9da8 190 //***************************************************
nerit 0:b0a79a3a9da8 191 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
nerit 0:b0a79a3a9da8 192 if(OnOffPin==1){
nerit 0:b0a79a3a9da8 193 if(ChangeStatusOfOnOffPinToHigh==1){
nerit 0:b0a79a3a9da8 194 ChangeStatusOfOnOffPinToHigh=0;
nerit 0:b0a79a3a9da8 195 OnOffPinTimerToHigh=millis();
nerit 0:b0a79a3a9da8 196 }else if((millis()-OnOffPinTimerToHigh)>VibrationTimer){
nerit 0:b0a79a3a9da8 197 if(ChangeFromOffToOn==1){
nerit 0:b0a79a3a9da8 198 uSWheel.reset();
nerit 0:b0a79a3a9da8 199 uSMotor.reset();
nerit 0:b0a79a3a9da8 200 MotorCorrection=(float)1;
nerit 0:b0a79a3a9da8 201 MotorSensorError=0;
nerit 0:b0a79a3a9da8 202 MotorSensorSecurityCheck=0;
nerit 0:b0a79a3a9da8 203 StopTheMotorWeGoTooSlow=0;
nerit 0:b0a79a3a9da8 204 for (i=0; i<ConstArrayIns; i++) { // |
nerit 0:b0a79a3a9da8 205 ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 206 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 207 } // |
nerit 0:b0a79a3a9da8 208 ChangeFromOffToOn=0;
nerit 0:b0a79a3a9da8 209 }
nerit 0:b0a79a3a9da8 210 DC_brake=0;
nerit 0:b0a79a3a9da8 211 DC_prepare();
nerit 0:b0a79a3a9da8 212 SetMotorSpeed();
nerit 0:b0a79a3a9da8 213 }
nerit 0:b0a79a3a9da8 214 }else{
nerit 0:b0a79a3a9da8 215 ChangeStatusOfOnOffPinToHigh=1;
nerit 0:b0a79a3a9da8 216 for (i=0; i<ConstArrayIns; i++) {
nerit 0:b0a79a3a9da8 217 ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 218 ReadMotorArray.push((int)1); // | this fills the motor sensor array with "1"
nerit 0:b0a79a3a9da8 219 }
nerit 0:b0a79a3a9da8 220 ChangeFromOffToOn=1;
nerit 0:b0a79a3a9da8 221 OffTheMotor();
nerit 0:b0a79a3a9da8 222 }
nerit 0:b0a79a3a9da8 223 }
nerit 0:b0a79a3a9da8 224
nerit 0:b0a79a3a9da8 225 //***************************************************
nerit 0:b0a79a3a9da8 226 //***************************************************
nerit 0:b0a79a3a9da8 227 //***************************************************
nerit 0:b0a79a3a9da8 228 int main() {
nerit 0:b0a79a3a9da8 229 wait(1);
nerit 0:b0a79a3a9da8 230 millisStart();
nerit 0:b0a79a3a9da8 231 uSMotor.start();
nerit 0:b0a79a3a9da8 232 uSWheel.start();
nerit 0:b0a79a3a9da8 233 WheelToMotorRatio=(float)(180/18); //RUOTA FONICA > MOTORE
nerit 0:b0a79a3a9da8 234 MotorPwmPin.period_us(2000); // freq 1khz
nerit 0:b0a79a3a9da8 235 MotorPwmPin.write(0.0f); // duty cycle=off
nerit 0:b0a79a3a9da8 236 OffTheMotor();
nerit 0:b0a79a3a9da8 237 if(MachineType=="electric"){
nerit 0:b0a79a3a9da8 238 MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds
nerit 0:b0a79a3a9da8 239 }
nerit 0:b0a79a3a9da8 240 for (i=0; i<ConstArrayIns; i++) {
nerit 0:b0a79a3a9da8 241 ReadWheelArray.push((int)1);
nerit 0:b0a79a3a9da8 242 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 243 }
nerit 0:b0a79a3a9da8 244 // pc.printf("Hello World");
nerit 0:b0a79a3a9da8 245 // pc.printf("%d",millis());
nerit 0:b0a79a3a9da8 246
nerit 0:b0a79a3a9da8 247 WheelSensorPin.rise(&ReadWheelInterrupt); //interrupt for wheel sensor, trigs when changes from 0 to 1
nerit 0:b0a79a3a9da8 248 MotorSensorPin.rise(&ReadMotorInterrupt); //interrupt for motor sensor, trigs when changes from 0 to 1
nerit 0:b0a79a3a9da8 249
nerit 0:b0a79a3a9da8 250
nerit 0:b0a79a3a9da8 251 //Analog input from 0.0 to 3.3V
nerit 0:b0a79a3a9da8 252 motorPID.setInputLimits(0, 42000); //70*secondosinaminute*trigsperwheelrevolutions
nerit 0:b0a79a3a9da8 253 //Pwm output from 0.0 to 1.0
nerit 0:b0a79a3a9da8 254 motorPID.setOutputLimits(0.0f, 1.0f);
nerit 0:b0a79a3a9da8 255 motorPID.setMode(1);
nerit 0:b0a79a3a9da8 256 //If there's a bias.
nerit 0:b0a79a3a9da8 257 //motorPID.setBias(0.3);
nerit 0:b0a79a3a9da8 258 //motorPID.setMode(AUTO);
nerit 0:b0a79a3a9da8 259 //We want the process variable to be 1.7V
nerit 0:b0a79a3a9da8 260 motorPID.setSetPoint(21000);
nerit 0:b0a79a3a9da8 261
nerit 0:b0a79a3a9da8 262 //***************************************************
nerit 0:b0a79a3a9da8 263 //***************************************************
nerit 0:b0a79a3a9da8 264 //***************************************************
nerit 0:b0a79a3a9da8 265 while (1){
nerit 0:b0a79a3a9da8 266 SwitchOnOff();
nerit 0:b0a79a3a9da8 267 }
nerit 0:b0a79a3a9da8 268 }