Francesco Pistone / Mbed 2 deprecated Viaro_SpandiConcime_V3_inizio

copia12092018

Dependencies:   mbed

main.cpp@9:6fb26643eecd, 2018-09-18 (annotated)

Committer:
francescopistone
Date:
Tue Sep 18 13:29:34 2018 +0000
Revision:
9:6fb26643eecd
Parent:
8:b01d4cb1857f 
con il solo fattore proporzionale, funziona

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){
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 }