Francesco Pistone
/
Viaro_SpandiConcime_V3_inizio
copia12092018
main.cpp@13:4d6114864f2d, 2018-11-16 (annotated)
- Committer:
- root@developer-sjc-indigo-compiler.local.mbed.org
- Date:
- Fri Nov 16 10:55:45 2018 +0000
- Revision:
- 13:4d6114864f2d
- Parent:
- 9:6fb26643eecd
Added tag fine for changeset dde73cf20353
Who changed what in which revision?
User | Revision | Line number | New 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){ |
viaromassimo | 4:1643fea75703 | 7 | SDmotorInA=1; |
viaromassimo | 4:1643fea75703 | 8 | SDmotorInB=1; |
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 | //*************************************************** |
viaromassimo | 4:1643fea75703 | 32 | void ResetData(){ |
viaromassimo | 4:1643fea75703 | 33 | ReadWheelArray.clear(); |
viaromassimo | 4:1643fea75703 | 34 | ReadMotorArray.clear(); |
viaromassimo | 4:1643fea75703 | 35 | ElapsedM=0; |
viaromassimo | 4:1643fea75703 | 36 | ElapsedW=0; |
viaromassimo | 4:1643fea75703 | 37 | TempReadM=0; |
viaromassimo | 4:1643fea75703 | 38 | TempReadW=0; |
viaromassimo | 4:1643fea75703 | 39 | WheelData=0; |
viaromassimo | 4:1643fea75703 | 40 | MotorData=0; |
viaromassimo | 4:1643fea75703 | 41 | motorPID.reset(); |
viaromassimo | 4:1643fea75703 | 42 | WantedMotorSpeed=0; |
viaromassimo | 4:1643fea75703 | 43 | ReadMotorSpeed=0; |
viaromassimo | 4:1643fea75703 | 44 | motorPID.setProcessValue(0); |
viaromassimo | 4:1643fea75703 | 45 | } |
viaromassimo | 4:1643fea75703 | 46 | |
viaromassimo | 4:1643fea75703 | 47 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 48 | void OffTheMotor(){ |
viaromassimo | 4:1643fea75703 | 49 | MotorPwmPin.write(0); // duty cycle=off - off the motor |
viaromassimo | 4:1643fea75703 | 50 | DC_brake=1; |
viaromassimo | 4:1643fea75703 | 51 | DC_prepare(); |
viaromassimo | 4:1643fea75703 | 52 | SecurityStop=1; |
viaromassimo | 4:1643fea75703 | 53 | ResetData(); |
nerit | 0:b0a79a3a9da8 | 54 | } |
nerit | 0:b0a79a3a9da8 | 55 | |
nerit | 0:b0a79a3a9da8 | 56 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 57 | void ReadWheelInterrupt(){ //interrupt that read the wheel sensor |
viaromassimo | 4:1643fea75703 | 58 | TempReadW=uSWheel.read_us(); |
viaromassimo | 4:1643fea75703 | 59 | if(TempReadW>200){ //thi s filter is used to avoid double readings |
viaromassimo | 4:1643fea75703 | 60 | if(TempReadW>800000){ |
viaromassimo | 4:1643fea75703 | 61 | TempReadW=800000; |
viaromassimo | 4:1643fea75703 | 62 | } |
viaromassimo | 4:1643fea75703 | 63 | ReadWheelArray.push(TempReadW); |
viaromassimo | 4:1643fea75703 | 64 | uSWheel.reset(); |
viaromassimo | 4:1643fea75703 | 65 | WheelData=1; |
viaromassimo | 4:1643fea75703 | 66 | } |
nerit | 0:b0a79a3a9da8 | 67 | } |
nerit | 0:b0a79a3a9da8 | 68 | |
nerit | 0:b0a79a3a9da8 | 69 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 70 | void ReadMotorInterrupt(){ //interrupt that read the motor sensor |
viaromassimo | 4:1643fea75703 | 71 | TempReadM=uSMotor.read_us(); |
viaromassimo | 4:1643fea75703 | 72 | if(TempReadM>200){ //thi s filter is used to avoid double readings, since the code takes 10 to 40 uS to cycle |
viaromassimo | 4:1643fea75703 | 73 | if(TempReadM>100000){ |
viaromassimo | 4:1643fea75703 | 74 | TempReadM=100000; |
viaromassimo | 4:1643fea75703 | 75 | } |
viaromassimo | 3:1cd8d048cd4a | 76 | ReadMotorArray.push(TempReadM); |
viaromassimo | 3:1cd8d048cd4a | 77 | uSMotor.reset(); |
viaromassimo | 4:1643fea75703 | 78 | MotorData=1; |
viaromassimo | 4:1643fea75703 | 79 | } |
nerit | 0:b0a79a3a9da8 | 80 | } |
nerit | 0:b0a79a3a9da8 | 81 | |
nerit | 0:b0a79a3a9da8 | 82 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 83 | void CheckPercentSwitch(){ //this below checks the add/take off Percent switches, with the control of the vibrations applied |
viaromassimo | 4:1643fea75703 | 84 | if(PlusPercentPin==1){ |
viaromassimo | 4:1643fea75703 | 85 | if(ChangeStatusOfPlusPercentPinToHigh==1){ |
viaromassimo | 4:1643fea75703 | 86 | ChangeStatusOfPlusPercentPinToHigh=0; |
viaromassimo | 4:1643fea75703 | 87 | vibPlusToHSwitch.start(); |
viaromassimo | 4:1643fea75703 | 88 | }else if(vibPlusToHSwitch.read_ms()>VibrationTimer){ |
viaromassimo | 4:1643fea75703 | 89 | vibPlusToHSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 90 | vibPlusToLSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 91 | vibPlusToLSwitch.reset(); |
viaromassimo | 4:1643fea75703 | 92 | ChangeStatusOfPlusPercentPinToLow=1; |
viaromassimo | 4:1643fea75703 | 93 | PercentPlus=1; |
viaromassimo | 4:1643fea75703 | 94 | } |
viaromassimo | 4:1643fea75703 | 95 | }else{ |
viaromassimo | 4:1643fea75703 | 96 | if(ChangeStatusOfPlusPercentPinToLow==1){ |
viaromassimo | 4:1643fea75703 | 97 | ChangeStatusOfPlusPercentPinToLow=0; |
viaromassimo | 4:1643fea75703 | 98 | vibPlusToLSwitch.start(); |
viaromassimo | 4:1643fea75703 | 99 | }else if(vibPlusToLSwitch.read_ms()>VibrationTimer){ |
viaromassimo | 4:1643fea75703 | 100 | vibPlusToLSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 101 | vibPlusToHSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 102 | vibPlusToHSwitch.reset(); |
viaromassimo | 4:1643fea75703 | 103 | ChangeStatusOfPlusPercentPinToHigh=1; |
viaromassimo | 4:1643fea75703 | 104 | PercentPlus=0; |
viaromassimo | 4:1643fea75703 | 105 | } |
nerit | 0:b0a79a3a9da8 | 106 | } |
nerit | 0:b0a79a3a9da8 | 107 | |
viaromassimo | 4:1643fea75703 | 108 | if(MinusPercentPin==1){ |
viaromassimo | 4:1643fea75703 | 109 | if(ChangeStatusOfMinusPercentPinToHigh==1){ |
viaromassimo | 4:1643fea75703 | 110 | ChangeStatusOfMinusPercentPinToHigh=0; |
viaromassimo | 4:1643fea75703 | 111 | vibMinusToHSwitch.start(); |
viaromassimo | 4:1643fea75703 | 112 | }else if(vibMinusToHSwitch.read_ms()>VibrationTimer){ |
viaromassimo | 4:1643fea75703 | 113 | vibMinusToHSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 114 | vibMinusToLSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 115 | vibMinusToLSwitch.reset(); |
viaromassimo | 4:1643fea75703 | 116 | ChangeStatusOfMinusPercentPinToLow=1; |
viaromassimo | 4:1643fea75703 | 117 | PercentMinus=1; |
viaromassimo | 4:1643fea75703 | 118 | } |
viaromassimo | 4:1643fea75703 | 119 | }else{ |
viaromassimo | 4:1643fea75703 | 120 | if(ChangeStatusOfMinusPercentPinToLow==1){ |
viaromassimo | 4:1643fea75703 | 121 | ChangeStatusOfMinusPercentPinToLow=0; |
viaromassimo | 4:1643fea75703 | 122 | vibMinusToLSwitch.start(); |
viaromassimo | 4:1643fea75703 | 123 | }else if(vibMinusToLSwitch.read_ms()>VibrationTimer){ |
viaromassimo | 4:1643fea75703 | 124 | vibMinusToLSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 125 | vibMinusToHSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 126 | vibMinusToHSwitch.reset(); |
viaromassimo | 4:1643fea75703 | 127 | ChangeStatusOfMinusPercentPinToHigh=1; |
viaromassimo | 4:1643fea75703 | 128 | PercentMinus=0; |
viaromassimo | 4:1643fea75703 | 129 | } |
nerit | 0:b0a79a3a9da8 | 130 | } |
nerit | 0:b0a79a3a9da8 | 131 | |
viaromassimo | 4:1643fea75703 | 132 | if(PercentPlus==0 && PercentMinus==1){ |
viaromassimo | 4:1643fea75703 | 133 | Percent=1.1f; |
viaromassimo | 4:1643fea75703 | 134 | } |
viaromassimo | 4:1643fea75703 | 135 | if(PercentPlus==1 && PercentMinus==0){ |
viaromassimo | 4:1643fea75703 | 136 | Percent=0.9f; |
viaromassimo | 4:1643fea75703 | 137 | } |
viaromassimo | 4:1643fea75703 | 138 | if(PercentPlus==1 && PercentMinus==1){ |
viaromassimo | 4:1643fea75703 | 139 | Percent=1.0f; |
viaromassimo | 4:1643fea75703 | 140 | } |
viaromassimo | 4:1643fea75703 | 141 | if(PercentPlus==0 && PercentMinus==0){ |
viaromassimo | 4:1643fea75703 | 142 | Percent=1.0f; |
nerit | 0:b0a79a3a9da8 | 143 | //GENERATE AN ERROR SOMEHOW |
viaromassimo | 4:1643fea75703 | 144 | } |
viaromassimo | 4:1643fea75703 | 145 | } |
viaromassimo | 4:1643fea75703 | 146 | |
viaromassimo | 4:1643fea75703 | 147 | //*************************************************** |
viaromassimo | 4:1643fea75703 | 148 | void writelog(){ |
nerit | 0:b0a79a3a9da8 | 149 | } |
nerit | 0:b0a79a3a9da8 | 150 | |
nerit | 0:b0a79a3a9da8 | 151 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 152 | void CheckMotorCorrection(){ |
viaromassimo | 4:1643fea75703 | 153 | if(MotorSensorError==0 && SecurityStop==0){ |
viaromassimo | 4:1643fea75703 | 154 | CheckPercentSwitch(); |
viaromassimo | 4:1643fea75703 | 155 | WantedMotorSpeed=(float)((60000000/(ElapsedW*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent); // =(( WHEEL_REVOLUTIONS/MINUTES )* RATIO CHANGE * +/- % ); rev/min |
francescopistone | 9:6fb26643eecd | 156 | if(WantedMotorSpeed < MinMotorSpeed){ |
francescopistone | 9:6fb26643eecd | 157 | //francesco 17/09/2018 |
francescopistone | 9:6fb26643eecd | 158 | //WantedMotorSpeed=0; |
francescopistone | 9:6fb26643eecd | 159 | WantedMotorSpeed = MinMotorSpeed; |
francescopistone | 9:6fb26643eecd | 160 | } |
francescopistone | 9:6fb26643eecd | 161 | if(WantedMotorSpeed > MaxMotorSpeed){ |
francescopistone | 9:6fb26643eecd | 162 | WantedMotorSpeed = MaxMotorSpeed; |
viaromassimo | 4:1643fea75703 | 163 | } |
viaromassimo | 4:1643fea75703 | 164 | |
viaromassimo | 4:1643fea75703 | 165 | if(MachineType=="electric"){ |
viaromassimo | 4:1643fea75703 | 166 | motorPID.setSetPoint(WantedMotorSpeed); |
viaromassimo | 4:1643fea75703 | 167 | motorPID.setProcessValue(ReadMotorSpeed); |
viaromassimo | 4:1643fea75703 | 168 | MotorSpeedCorrected=motorPID.compute(); |
viaromassimo | 4:1643fea75703 | 169 | if(WantedMotorSpeed!=0 && uSMotor.read_us()>1000000){ |
francescopistone | 9:6fb26643eecd | 170 | //francesco 17/09/2018 -->> test |
viaromassimo | 4:1643fea75703 | 171 | MotorSensorSecurityCheck=MotorSensorSecurityCheck++; |
viaromassimo | 4:1643fea75703 | 172 | if(MotorSensorSecurityCheck>50){ |
viaromassimo | 4:1643fea75703 | 173 | MotorSensorError=1; // OFF and ON to reset |
viaromassimo | 4:1643fea75703 | 174 | } |
viaromassimo | 4:1643fea75703 | 175 | }else{ |
viaromassimo | 4:1643fea75703 | 176 | MotorSensorSecurityCheck=0; |
viaromassimo | 4:1643fea75703 | 177 | } |
viaromassimo | 4:1643fea75703 | 178 | } |
viaromassimo | 4:1643fea75703 | 179 | |
viaromassimo | 4:1643fea75703 | 180 | if(MachineType=="idraulic"){ |
viaromassimo | 4:1643fea75703 | 181 | 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 |
viaromassimo | 4:1643fea75703 | 182 | } |
viaromassimo | 4:1643fea75703 | 183 | |
viaromassimo | 4:1643fea75703 | 184 | MotorPwmPin.write(MotorSpeedCorrected); |
francescopistone | 9:6fb26643eecd | 185 | //francesco 17/09/2018 -> test |
francescopistone | 9:6fb26643eecd | 186 | //MotorPwmPin.write(0.2f); |
francescopistone | 9:6fb26643eecd | 187 | //MotorPwmPin.write(0.9f); |
francescopistone | 9:6fb26643eecd | 188 | |
viaromassimo | 4:1643fea75703 | 189 | DC_brake=0; |
viaromassimo | 4:1643fea75703 | 190 | DC_prepare(); |
viaromassimo | 5:72bdc69d610d | 191 | #if defined(logActive) |
viaromassimo | 5:72bdc69d610d | 192 | pc.printf("\nMotorSpeedCorrected %f",MotorSpeedCorrected); |
viaromassimo | 5:72bdc69d610d | 193 | pc.printf(" - WantedMotorSpeed %d",WantedMotorSpeed); |
viaromassimo | 5:72bdc69d610d | 194 | pc.printf(" - ReadMotorSpeed %d",ReadMotorSpeed); |
viaromassimo | 5:72bdc69d610d | 195 | pc.printf(" - ElapsedW %d",ElapsedW); |
viaromassimo | 5:72bdc69d610d | 196 | pc.printf(" - TempReadW %d",TempReadW); |
viaromassimo | 5:72bdc69d610d | 197 | pc.printf(" - ElapsedM %d",ElapsedM); |
viaromassimo | 5:72bdc69d610d | 198 | pc.printf(" - TempReadM %d",TempReadM); |
viaromassimo | 5:72bdc69d610d | 199 | pc.printf(" - Plus %d",PercentPlus); |
viaromassimo | 5:72bdc69d610d | 200 | pc.printf(" - Minus %d",PercentMinus); |
viaromassimo | 5:72bdc69d610d | 201 | pc.printf(" - Security %d",SecurityStop); |
viaromassimo | 5:72bdc69d610d | 202 | #endif |
viaromassimo | 4:1643fea75703 | 203 | }else{ |
viaromassimo | 4:1643fea75703 | 204 | OffTheMotor(); |
nerit | 0:b0a79a3a9da8 | 205 | } |
nerit | 0:b0a79a3a9da8 | 206 | } |
nerit | 0:b0a79a3a9da8 | 207 | |
nerit | 0:b0a79a3a9da8 | 208 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 209 | void CheckTimeElapsedFromLastTrig(){ //check time Elapsed from last wheel trig |
viaromassimo | 4:1643fea75703 | 210 | 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 |
viaromassimo | 4:1643fea75703 | 211 | OffTheMotor(); |
viaromassimo | 4:1643fea75703 | 212 | }else{ |
viaromassimo | 4:1643fea75703 | 213 | SecurityStop=0; |
nerit | 0:b0a79a3a9da8 | 214 | } |
nerit | 0:b0a79a3a9da8 | 215 | } |
nerit | 0:b0a79a3a9da8 | 216 | |
nerit | 0:b0a79a3a9da8 | 217 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 218 | void SetMotorSpeed(){ |
viaromassimo | 4:1643fea75703 | 219 | CheckTimeElapsedFromLastTrig(); |
viaromassimo | 4:1643fea75703 | 220 | |
viaromassimo | 4:1643fea75703 | 221 | if(WheelData==1){ |
viaromassimo | 4:1643fea75703 | 222 | ElapsedW=0; |
viaromassimo | 4:1643fea75703 | 223 | a=0; |
viaromassimo | 4:1643fea75703 | 224 | for(i=0; i<(ReadWheelArray.size()-1); i++){ |
viaromassimo | 4:1643fea75703 | 225 | if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){ //avoid reading of 1 or 0 values in the array |
viaromassimo | 4:1643fea75703 | 226 | a++; //number of all the valid readings |
viaromassimo | 4:1643fea75703 | 227 | ElapsedW=ElapsedW+ReadWheelArray[i]; //all the valid readings are summed |
viaromassimo | 4:1643fea75703 | 228 | } |
viaromassimo | 4:1643fea75703 | 229 | } |
viaromassimo | 4:1643fea75703 | 230 | ElapsedW=ElapsedW/a; //calculation of avarage of the valid readings |
viaromassimo | 4:1643fea75703 | 231 | WheelData=0; |
viaromassimo | 3:1cd8d048cd4a | 232 | } |
viaromassimo | 4:1643fea75703 | 233 | |
viaromassimo | 4:1643fea75703 | 234 | if(MotorData==1){ |
viaromassimo | 4:1643fea75703 | 235 | ElapsedM=0; |
viaromassimo | 4:1643fea75703 | 236 | a=0; |
viaromassimo | 4:1643fea75703 | 237 | for(i=0; i<(ReadMotorArray.size()-1); i++){ |
viaromassimo | 4:1643fea75703 | 238 | if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){ //avoid reading of 1 or 0 values in the array |
viaromassimo | 4:1643fea75703 | 239 | a++; //number of all the valid readings |
viaromassimo | 4:1643fea75703 | 240 | ElapsedM=ElapsedM+ReadMotorArray[i]; //all the valid readings are summed |
viaromassimo | 4:1643fea75703 | 241 | } |
viaromassimo | 4:1643fea75703 | 242 | } |
viaromassimo | 4:1643fea75703 | 243 | ElapsedM=ElapsedM/a; |
viaromassimo | 4:1643fea75703 | 244 | MotorData=0; |
viaromassimo | 4:1643fea75703 | 245 | } |
viaromassimo | 4:1643fea75703 | 246 | |
viaromassimo | 4:1643fea75703 | 247 | if(ElapsedM==0){ |
viaromassimo | 4:1643fea75703 | 248 | ReadMotorSpeed=0; |
viaromassimo | 3:1cd8d048cd4a | 249 | }else{ |
francescopistone | 7:dc3cb1a5764b | 250 | //francesco 14/09/2018 -->> il rapporto di riduzione è già stato considerato |
francescopistone | 9:6fb26643eecd | 251 | ReadMotorSpeed=float(60000000/ElapsedM); // rev/min |
francescopistone | 9:6fb26643eecd | 252 | //ReadMotorSpeed=float(60000000/(ElapsedM*4.0f)); // rev/min |
francescopistone | 9:6fb26643eecd | 253 | |
francescopistone | 9:6fb26643eecd | 254 | if(ReadMotorSpeed < MinMotorSpeed){ |
francescopistone | 9:6fb26643eecd | 255 | ReadMotorSpeed = MinMotorSpeed; |
francescopistone | 9:6fb26643eecd | 256 | } |
francescopistone | 9:6fb26643eecd | 257 | if(ReadMotorSpeed > MaxMotorSpeed){ |
francescopistone | 9:6fb26643eecd | 258 | ReadMotorSpeed = MaxMotorSpeed; |
francescopistone | 9:6fb26643eecd | 259 | } |
francescopistone | 8:b01d4cb1857f | 260 | |
viaromassimo | 3:1cd8d048cd4a | 261 | } |
nerit | 0:b0a79a3a9da8 | 262 | } |
nerit | 0:b0a79a3a9da8 | 263 | |
nerit | 0:b0a79a3a9da8 | 264 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 265 | 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 |
viaromassimo | 3:1cd8d048cd4a | 266 | // pc.printf("\nTIME %d ",uSCycle.read_us()); |
viaromassimo | 3:1cd8d048cd4a | 267 | // uSCycle.reset(); |
viaromassimo | 4:1643fea75703 | 268 | if(OnOffPin==0){ |
viaromassimo | 4:1643fea75703 | 269 | if(ChangeStatusOfOnOffPinToHigh==1){ |
viaromassimo | 4:1643fea75703 | 270 | ChangeStatusOfOnOffPinToHigh=0; |
viaromassimo | 4:1643fea75703 | 271 | vibOnOffSwitch.start(); |
viaromassimo | 4:1643fea75703 | 272 | }else if(vibOnOffSwitch.read_ms()>VibrationTimer){ |
viaromassimo | 4:1643fea75703 | 273 | vibOnOffSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 274 | if(ChangeFromOffToOn==1){ |
viaromassimo | 4:1643fea75703 | 275 | ChangeFromOffToOn=0; |
viaromassimo | 4:1643fea75703 | 276 | } |
viaromassimo | 4:1643fea75703 | 277 | DC_brake=0; |
viaromassimo | 4:1643fea75703 | 278 | DC_prepare(); |
viaromassimo | 4:1643fea75703 | 279 | SetMotorSpeed(); |
viaromassimo | 4:1643fea75703 | 280 | } |
viaromassimo | 4:1643fea75703 | 281 | }else{ |
viaromassimo | 4:1643fea75703 | 282 | ChangeStatusOfOnOffPinToHigh=1; |
viaromassimo | 4:1643fea75703 | 283 | vibOnOffSwitch.stop(); |
viaromassimo | 4:1643fea75703 | 284 | vibOnOffSwitch.reset(); |
viaromassimo | 4:1643fea75703 | 285 | ChangeFromOffToOn=1; |
nerit | 0:b0a79a3a9da8 | 286 | MotorSensorError=0; |
viaromassimo | 4:1643fea75703 | 287 | OffTheMotor(); |
nerit | 0:b0a79a3a9da8 | 288 | } |
nerit | 0:b0a79a3a9da8 | 289 | } |
nerit | 0:b0a79a3a9da8 | 290 | |
nerit | 0:b0a79a3a9da8 | 291 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 292 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 293 | //*************************************************** |
viaromassimo | 4:1643fea75703 | 294 | int main(){ |
viaromassimo | 4:1643fea75703 | 295 | wait(1); |
viaromassimo | 4:1643fea75703 | 296 | uSMotor.start(); |
viaromassimo | 4:1643fea75703 | 297 | uSWheel.start(); |
francescopistone | 9:6fb26643eecd | 298 | //francesco 14/09/2018 -->> alzo il limite |
francescopistone | 9:6fb26643eecd | 299 | //motorPID.setInputLimits(0, 1000000); -->> il test non ha buoni frutti, rimetto l'originale 10000 |
francescopistone | 9:6fb26643eecd | 300 | motorPID.setInputLimits(1000, 10000); //70*secondosinaminute*trigsperwheelrevolutions |
francescopistone | 9:6fb26643eecd | 301 | |
francescopistone | 9:6fb26643eecd | 302 | motorPID.setOutputLimits(0.0f, 1.0f); |
francescopistone | 9:6fb26643eecd | 303 | motorPID.setMode(1); |
francescopistone | 7:dc3cb1a5764b | 304 | //MotorPwmPin.period_us(2000); // freq 1khz |
francescopistone | 7:dc3cb1a5764b | 305 | //francesco 13/09/2018 -->> periodo sbagliato se si vuole una frequenza di un khz |
francescopistone | 7:dc3cb1a5764b | 306 | MotorPwmPin.period_us(1000); |
viaromassimo | 4:1643fea75703 | 307 | MotorPwmPin.write(0.0f); // duty cycle=off |
viaromassimo | 4:1643fea75703 | 308 | OffTheMotor(); |
nerit | 0:b0a79a3a9da8 | 309 | MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds |
viaromassimo | 4:1643fea75703 | 310 | ReadWheelArray.clear(); |
viaromassimo | 4:1643fea75703 | 311 | ReadMotorArray.clear(); |
viaromassimo | 5:72bdc69d610d | 312 | #if defined(logActive) |
viaromassimo | 5:72bdc69d610d | 313 | pc.printf("\n\n Salute, Capo!\n\n"); |
viaromassimo | 5:72bdc69d610d | 314 | #endif |
viaromassimo | 4:1643fea75703 | 315 | WheelSensorPin.rise(&ReadWheelInterrupt); //interrupt for wheel sensor, trigs when changes from 0 to 1 |
viaromassimo | 4:1643fea75703 | 316 | MotorSensorPin.rise(&ReadMotorInterrupt); //interrupt for motor sensor, trigs when changes from 0 to 1 |
viaromassimo | 4:1643fea75703 | 317 | |
francescopistone | 7:dc3cb1a5764b | 318 | //francesco 14/09/2018 -->> non capisco perchè fornire un set point fisso |
francescopistone | 7:dc3cb1a5764b | 319 | //motorPID.setSetPoint(1000); |
nerit | 0:b0a79a3a9da8 | 320 | |
nerit | 0:b0a79a3a9da8 | 321 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 322 | //*************************************************** |
nerit | 0:b0a79a3a9da8 | 323 | //*************************************************** |
viaromassimo | 4:1643fea75703 | 324 | while(1){ |
viaromassimo | 4:1643fea75703 | 325 | SwitchOnOff(); |
viaromassimo | 4:1643fea75703 | 326 | } |
nerit | 0:b0a79a3a9da8 | 327 | } |