Francesco Pistone / Mbed 2 deprecated Viaro_SpandiConcime_V3_inizio

copia12092018

Dependencies:   mbed

main.cpp@3:1cd8d048cd4a, 2018-06-19 (annotated)

Committer:
viaromassimo
Date:
Tue Jun 19 16:47:15 2018 +0000
Revision:
3:1cd8d048cd4a
Parent:
2:5111c6c0639e
Child:
4:1643fea75703
- added control for minimal speed (below wich it stops); - added ramp for motor startup

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();
viaromassimo 3:1cd8d048cd4a 49 if(TempReadM>200){ //thi s filter is used to avoid double readings, since the code takes 10 to 40 uS to cycle
viaromassimo 3:1cd8d048cd4a 50 ReadMotorArray.push(TempReadM);
viaromassimo 3:1cd8d048cd4a 51 uSMotor.reset();
viaromassimo 3:1cd8d048cd4a 52 }
nerit 0:b0a79a3a9da8 53 }
nerit 0:b0a79a3a9da8 54
nerit 0:b0a79a3a9da8 55 //***************************************************
nerit 0:b0a79a3a9da8 56 void CheckPercentSwitch(){ //this below checks the add/take off Percent switches, with the control of the vibrations applied
nerit 0:b0a79a3a9da8 57 if(PlusPercentPin==1){
nerit 0:b0a79a3a9da8 58 if(ChangeStatusOfPlusPercentPinToHigh==1){
nerit 0:b0a79a3a9da8 59 ChangeStatusOfPlusPercentPinToHigh=0;
nerit 0:b0a79a3a9da8 60 PlusPercentPinTimerToHigh=millis();
nerit 0:b0a79a3a9da8 61 }else if((millis()-PlusPercentPinTimerToHigh)>VibrationTimer){
nerit 0:b0a79a3a9da8 62 ChangeStatusOfPlusPercentPinToLow=1;
nerit 0:b0a79a3a9da8 63 PercentPlus=1;
nerit 0:b0a79a3a9da8 64 }
nerit 0:b0a79a3a9da8 65 }else{
nerit 0:b0a79a3a9da8 66 if(ChangeStatusOfPlusPercentPinToLow==1){
nerit 0:b0a79a3a9da8 67 ChangeStatusOfPlusPercentPinToLow=0;
nerit 0:b0a79a3a9da8 68 PlusPercentPinTimerToLow=millis();
nerit 0:b0a79a3a9da8 69 }else if((millis()-PlusPercentPinTimerToLow)>VibrationTimer){
nerit 0:b0a79a3a9da8 70 ChangeStatusOfPlusPercentPinToHigh=1;
nerit 0:b0a79a3a9da8 71 PercentPlus=0;
nerit 0:b0a79a3a9da8 72 }
nerit 0:b0a79a3a9da8 73 }
nerit 0:b0a79a3a9da8 74
nerit 0:b0a79a3a9da8 75 if(MinusPercentPin==1){
nerit 0:b0a79a3a9da8 76 if(ChangeStatusOfMinusPercentPinToHigh==1){
nerit 0:b0a79a3a9da8 77 ChangeStatusOfMinusPercentPinToHigh=0;
nerit 0:b0a79a3a9da8 78 MinusPercentPinTimerToHigh=millis();
nerit 0:b0a79a3a9da8 79 }else if((millis()-MinusPercentPinTimerToHigh)>VibrationTimer){
nerit 0:b0a79a3a9da8 80 ChangeStatusOfMinusPercentPinToLow=1;
nerit 0:b0a79a3a9da8 81 PercentMinus=1;
nerit 0:b0a79a3a9da8 82 }
nerit 0:b0a79a3a9da8 83 }else{
nerit 0:b0a79a3a9da8 84 if(ChangeStatusOfMinusPercentPinToLow==1){
nerit 0:b0a79a3a9da8 85 ChangeStatusOfMinusPercentPinToLow=0;
nerit 0:b0a79a3a9da8 86 MinusPercentPinTimerToLow=millis();
nerit 0:b0a79a3a9da8 87 }else if((millis()-MinusPercentPinTimerToLow)>VibrationTimer){
nerit 0:b0a79a3a9da8 88 ChangeStatusOfMinusPercentPinToHigh=1;
nerit 0:b0a79a3a9da8 89 PercentMinus=0;
nerit 0:b0a79a3a9da8 90 }
nerit 0:b0a79a3a9da8 91 }
nerit 0:b0a79a3a9da8 92
nerit 0:b0a79a3a9da8 93 if(PercentPlus==1 && PercentMinus==0){
nerit 0:b0a79a3a9da8 94 Percent=(float)1.1;
nerit 0:b0a79a3a9da8 95 }
nerit 0:b0a79a3a9da8 96 if(PercentPlus==0 && PercentMinus==1){
nerit 0:b0a79a3a9da8 97 Percent=(float)0.9;
nerit 0:b0a79a3a9da8 98 }
nerit 0:b0a79a3a9da8 99 if(PercentPlus==0 && PercentMinus==0){
nerit 0:b0a79a3a9da8 100 Percent=(float)1.0;
nerit 0:b0a79a3a9da8 101 }
nerit 0:b0a79a3a9da8 102 if(PercentPlus==1 && PercentMinus==1){
nerit 0:b0a79a3a9da8 103 Percent=(float)1.0;
nerit 0:b0a79a3a9da8 104 //GENERATE AN ERROR SOMEHOW
nerit 0:b0a79a3a9da8 105 }
nerit 0:b0a79a3a9da8 106 }
nerit 0:b0a79a3a9da8 107
nerit 0:b0a79a3a9da8 108 //***************************************************
nerit 0:b0a79a3a9da8 109 void CheckMotorCorrection(){
nerit 0:b0a79a3a9da8 110 ElapsedM=0;
nerit 0:b0a79a3a9da8 111 a=0;
nerit 0:b0a79a3a9da8 112 for(i=0; i<(ReadMotorArray.size()-1); i++){
nerit 0:b0a79a3a9da8 113 if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){ //avoid reading of 1 or 0 values in the array
nerit 0:b0a79a3a9da8 114 a++; //number of all the valid readings
nerit 0:b0a79a3a9da8 115 ElapsedM=ElapsedM+ReadMotorArray[i]; //all the valid readings are summed
nerit 0:b0a79a3a9da8 116 }
nerit 0:b0a79a3a9da8 117 }
nerit 0:b0a79a3a9da8 118 ElapsedM=ElapsedM/a;
viaromassimo 3:1cd8d048cd4a 119 if((StopTheMotorWeGoTooSlow==0) && WantedMotorSpeed>MinMotorSpeed && (ElapsedM!=1) && uSMotor.read_us()>(2*ElapsedM)){
nerit 0:b0a79a3a9da8 120 MotorSensorSecurityCheck=MotorSensorSecurityCheck++;
nerit 0:b0a79a3a9da8 121 if(MotorSensorSecurityCheck>50){
nerit 0:b0a79a3a9da8 122 MotorSensorError=1;
nerit 0:b0a79a3a9da8 123 }
viaromassimo 3:1cd8d048cd4a 124 pc.printf("\nMotorSensorSecurityCheck %d ",MotorSensorSecurityCheck);
viaromassimo 3:1cd8d048cd4a 125 pc.printf("\nMotorSensorError %d ",MotorSensorError);
nerit 0:b0a79a3a9da8 126 }else{
viaromassimo 3:1cd8d048cd4a 127 if(WantedMotorSpeed>MinMotorSpeed && OnOffPin==0){
nerit 0:b0a79a3a9da8 128 ReadMotorSpeed=float(60000000/(ElapsedM*4)); // rev/min
nerit 0:b0a79a3a9da8 129 motorPID.setSetPoint(WantedMotorSpeed);
nerit 0:b0a79a3a9da8 130 motorPID.setProcessValue(ReadMotorSpeed);
nerit 0:b0a79a3a9da8 131 MotorSpeedCorrected=motorPID.compute();
viaromassimo 3:1cd8d048cd4a 132 if(mSMotorRamp.read_ms()<1800 && MotorSpeedCorrected>0.25f){
viaromassimo 3:1cd8d048cd4a 133 MotorSpeedCorrected=0.25f;
viaromassimo 3:1cd8d048cd4a 134 // }else if(mSMotorRamp.read_ms()<2400 && MotorSpeedCorrected>0.60f){
viaromassimo 3:1cd8d048cd4a 135 // MotorSpeedCorrected=0.60f;
viaromassimo 3:1cd8d048cd4a 136 }else if(mSMotorRamp.read_ms()>1799){
viaromassimo 3:1cd8d048cd4a 137 mSMotorRamp.stop();
viaromassimo 3:1cd8d048cd4a 138 mSMotorRamp.reset();
viaromassimo 3:1cd8d048cd4a 139 }
viaromassimo 3:1cd8d048cd4a 140 }
viaromassimo 1:59c30e854dfb 141 pc.printf("\n\nMotorSpeedCorrectedF %f ",MotorSpeedCorrected);
viaromassimo 1:59c30e854dfb 142 pc.printf("\nWantedMotorSpeed %d ",WantedMotorSpeed);
nerit 0:b0a79a3a9da8 143 MotorSensorSecurityCheck=0;
nerit 0:b0a79a3a9da8 144 }
nerit 0:b0a79a3a9da8 145 }
nerit 0:b0a79a3a9da8 146
nerit 0:b0a79a3a9da8 147 //***************************************************
nerit 0:b0a79a3a9da8 148 void CheckTimeElapsedFromLastTrig(){ //check time Elapsed from last wheel trig
nerit 0:b0a79a3a9da8 149 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 150 OffTheMotor();
nerit 0:b0a79a3a9da8 151 }else{
nerit 0:b0a79a3a9da8 152 if(StopTheMotorWeGoTooSlow==1){
viaromassimo 3:1cd8d048cd4a 153 uSMotor.reset(); //reset the the uS of PreviousM read
nerit 0:b0a79a3a9da8 154 for (i=0; i<ConstArrayIns; i++) {
nerit 0:b0a79a3a9da8 155 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 156 MotorCorrection=1.0f;
nerit 0:b0a79a3a9da8 157 }
nerit 0:b0a79a3a9da8 158 }
nerit 0:b0a79a3a9da8 159 StopTheMotorWeGoTooSlow=0;
nerit 0:b0a79a3a9da8 160 }
nerit 0:b0a79a3a9da8 161 }
nerit 0:b0a79a3a9da8 162
nerit 0:b0a79a3a9da8 163 //***************************************************
nerit 0:b0a79a3a9da8 164 void SetMotorSpeed(){
viaromassimo 3:1cd8d048cd4a 165 // led=0;
nerit 0:b0a79a3a9da8 166 CheckTimeElapsedFromLastTrig();
nerit 0:b0a79a3a9da8 167 if(MotorSensorError==1){
nerit 0:b0a79a3a9da8 168 OffTheMotor();
nerit 0:b0a79a3a9da8 169 // GENERATE MOTOR SENSOR ERROR
nerit 0:b0a79a3a9da8 170 }
viaromassimo 3:1cd8d048cd4a 171 Elapsed=0;
viaromassimo 3:1cd8d048cd4a 172 a=0;
viaromassimo 3:1cd8d048cd4a 173 for(i=0; i<(ReadWheelArray.size()-1); i++){
viaromassimo 3:1cd8d048cd4a 174 if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){ //avoid reading of 1 or 0 values in the array
viaromassimo 3:1cd8d048cd4a 175 a++; //number of all the valid readings
viaromassimo 3:1cd8d048cd4a 176 Elapsed=Elapsed+ReadWheelArray[i]; //all the valid readings are summed
viaromassimo 3:1cd8d048cd4a 177 }
viaromassimo 3:1cd8d048cd4a 178 }
viaromassimo 3:1cd8d048cd4a 179 Elapsed=Elapsed/a; //calculation of avarage of the valid readings
viaromassimo 3:1cd8d048cd4a 180 if(uSWheel.read_us()<1000000){
viaromassimo 3:1cd8d048cd4a 181 StopTheMotorWeGoTooSlow=0;
viaromassimo 3:1cd8d048cd4a 182 }else{
viaromassimo 3:1cd8d048cd4a 183 ElapsedM=1;
viaromassimo 3:1cd8d048cd4a 184 uSWheel.reset();
viaromassimo 3:1cd8d048cd4a 185 uSMotor.reset();
viaromassimo 3:1cd8d048cd4a 186 MotorCorrection=(float)1;
viaromassimo 3:1cd8d048cd4a 187 MotorSensorError=0;
viaromassimo 3:1cd8d048cd4a 188 MotorSensorSecurityCheck=0;
viaromassimo 3:1cd8d048cd4a 189 StopTheMotorWeGoTooSlow=0;
viaromassimo 3:1cd8d048cd4a 190 for (i=0; i<ConstArrayIns; i++) { // |
viaromassimo 3:1cd8d048cd4a 191 ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1"
viaromassimo 3:1cd8d048cd4a 192 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
viaromassimo 3:1cd8d048cd4a 193 } // |
viaromassimo 3:1cd8d048cd4a 194 }
nerit 0:b0a79a3a9da8 195 if(StopTheMotorWeGoTooSlow==0){
nerit 0:b0a79a3a9da8 196 CheckPercentSwitch();
nerit 0:b0a79a3a9da8 197 // BELOW=(( WHEEL_REVOLUTIONS/MINUTES )* RATIO CHANGE * +/- % );
nerit 0:b0a79a3a9da8 198 WantedMotorSpeed=(float)((60000000/(Elapsed*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent); // rev/min
viaromassimo 3:1cd8d048cd4a 199 if(WantedMotorSpeed<MinMotorSpeed){
viaromassimo 3:1cd8d048cd4a 200 // ElapsedM=1;
viaromassimo 3:1cd8d048cd4a 201 // uSWheel.reset();
viaromassimo 3:1cd8d048cd4a 202 // uSMotor.reset();
viaromassimo 3:1cd8d048cd4a 203 MotorSensorSecurityCheck=0;
viaromassimo 3:1cd8d048cd4a 204 OffTheMotor();
viaromassimo 3:1cd8d048cd4a 205 StopTheMotorWeGoTooSlow=0;
viaromassimo 3:1cd8d048cd4a 206 }else{
viaromassimo 3:1cd8d048cd4a 207 if(MachineType=="idraulic"){
nerit 0:b0a79a3a9da8 208 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 3:1cd8d048cd4a 209 }
viaromassimo 3:1cd8d048cd4a 210 DC_brake=0;
viaromassimo 3:1cd8d048cd4a 211 DC_prepare();
viaromassimo 3:1cd8d048cd4a 212 MotorPwmPin.write(MotorSpeedCorrected);
viaromassimo 3:1cd8d048cd4a 213 // pc.printf("\nMotorSpeedCorrectedF %f ",MotorSpeedCorrected);
viaromassimo 3:1cd8d048cd4a 214 // led=1;
viaromassimo 3:1cd8d048cd4a 215 }
nerit 0:b0a79a3a9da8 216 }
nerit 0:b0a79a3a9da8 217 }
nerit 0:b0a79a3a9da8 218
nerit 0:b0a79a3a9da8 219 //***************************************************
nerit 0:b0a79a3a9da8 220 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 221 // pc.printf("\nTIME %d ",uSCycle.read_us());
viaromassimo 3:1cd8d048cd4a 222 // uSCycle.reset();
viaromassimo 3:1cd8d048cd4a 223 if(OnOffPin==0){
nerit 0:b0a79a3a9da8 224 if(ChangeStatusOfOnOffPinToHigh==1){
nerit 0:b0a79a3a9da8 225 ChangeStatusOfOnOffPinToHigh=0;
nerit 0:b0a79a3a9da8 226 OnOffPinTimerToHigh=millis();
nerit 0:b0a79a3a9da8 227 }else if((millis()-OnOffPinTimerToHigh)>VibrationTimer){
nerit 0:b0a79a3a9da8 228 if(ChangeFromOffToOn==1){
viaromassimo 3:1cd8d048cd4a 229 ElapsedM=1;
nerit 0:b0a79a3a9da8 230 uSWheel.reset();
nerit 0:b0a79a3a9da8 231 uSMotor.reset();
nerit 0:b0a79a3a9da8 232 MotorCorrection=(float)1;
nerit 0:b0a79a3a9da8 233 MotorSensorError=0;
nerit 0:b0a79a3a9da8 234 MotorSensorSecurityCheck=0;
nerit 0:b0a79a3a9da8 235 StopTheMotorWeGoTooSlow=0;
nerit 0:b0a79a3a9da8 236 for (i=0; i<ConstArrayIns; i++) { // |
nerit 0:b0a79a3a9da8 237 ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 238 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 239 } // |
nerit 0:b0a79a3a9da8 240 ChangeFromOffToOn=0;
nerit 0:b0a79a3a9da8 241 }
viaromassimo 3:1cd8d048cd4a 242 mSMotorRamp.start();
nerit 0:b0a79a3a9da8 243 DC_brake=0;
nerit 0:b0a79a3a9da8 244 DC_prepare();
nerit 0:b0a79a3a9da8 245 SetMotorSpeed();
nerit 0:b0a79a3a9da8 246 }
nerit 0:b0a79a3a9da8 247 }else{
nerit 0:b0a79a3a9da8 248 ChangeStatusOfOnOffPinToHigh=1;
nerit 0:b0a79a3a9da8 249 for (i=0; i<ConstArrayIns; i++) {
nerit 0:b0a79a3a9da8 250 ReadWheelArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 251 ReadMotorArray.push((int)1); // | this fills the motor sensor array with "1"
nerit 0:b0a79a3a9da8 252 }
nerit 0:b0a79a3a9da8 253 ChangeFromOffToOn=1;
nerit 0:b0a79a3a9da8 254 OffTheMotor();
viaromassimo 3:1cd8d048cd4a 255 mSMotorRamp.stop();
viaromassimo 3:1cd8d048cd4a 256 mSMotorRamp.reset();
viaromassimo 3:1cd8d048cd4a 257
nerit 0:b0a79a3a9da8 258 }
nerit 0:b0a79a3a9da8 259 }
nerit 0:b0a79a3a9da8 260
nerit 0:b0a79a3a9da8 261 //***************************************************
nerit 0:b0a79a3a9da8 262 //***************************************************
nerit 0:b0a79a3a9da8 263 //***************************************************
nerit 0:b0a79a3a9da8 264 int main() {
nerit 0:b0a79a3a9da8 265 wait(1);
nerit 0:b0a79a3a9da8 266 millisStart();
viaromassimo 3:1cd8d048cd4a 267 // uSCycle.start();
nerit 0:b0a79a3a9da8 268 uSMotor.start();
nerit 0:b0a79a3a9da8 269 uSWheel.start();
viaromassimo 2:5111c6c0639e 270 WheelToMotorRatio=(float)(1800/18); //RUOTA FONICA > MOTORE
nerit 0:b0a79a3a9da8 271 MotorPwmPin.period_us(2000); // freq 1khz
nerit 0:b0a79a3a9da8 272 MotorPwmPin.write(0.0f); // duty cycle=off
nerit 0:b0a79a3a9da8 273 OffTheMotor();
nerit 0:b0a79a3a9da8 274 if(MachineType=="electric"){
nerit 0:b0a79a3a9da8 275 MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds
nerit 0:b0a79a3a9da8 276 }
nerit 0:b0a79a3a9da8 277 for (i=0; i<ConstArrayIns; i++) {
nerit 0:b0a79a3a9da8 278 ReadWheelArray.push((int)1);
nerit 0:b0a79a3a9da8 279 ReadMotorArray.push((int)1); // | this fills the wheel sensor array with "1"
nerit 0:b0a79a3a9da8 280 }
viaromassimo 1:59c30e854dfb 281 pc.printf("Hello World");
viaromassimo 1:59c30e854dfb 282 pc.printf("%d",millis());
nerit 0:b0a79a3a9da8 283
nerit 0:b0a79a3a9da8 284 WheelSensorPin.rise(&ReadWheelInterrupt); //interrupt for wheel sensor, trigs when changes from 0 to 1
nerit 0:b0a79a3a9da8 285 MotorSensorPin.rise(&ReadMotorInterrupt); //interrupt for motor sensor, trigs when changes from 0 to 1
nerit 0:b0a79a3a9da8 286
nerit 0:b0a79a3a9da8 287
nerit 0:b0a79a3a9da8 288 //Analog input from 0.0 to 3.3V
viaromassimo 3:1cd8d048cd4a 289 motorPID.setInputLimits(0, 10000); //70*secondosinaminute*trigsperwheelrevolutions
nerit 0:b0a79a3a9da8 290 //Pwm output from 0.0 to 1.0
nerit 0:b0a79a3a9da8 291 motorPID.setOutputLimits(0.0f, 1.0f);
nerit 0:b0a79a3a9da8 292 motorPID.setMode(1);
nerit 0:b0a79a3a9da8 293 //If there's a bias.
nerit 0:b0a79a3a9da8 294 //motorPID.setBias(0.3);
nerit 0:b0a79a3a9da8 295 //motorPID.setMode(AUTO);
nerit 0:b0a79a3a9da8 296 //We want the process variable to be 1.7V
viaromassimo 3:1cd8d048cd4a 297 motorPID.setSetPoint(1000);
nerit 0:b0a79a3a9da8 298
nerit 0:b0a79a3a9da8 299 //***************************************************
nerit 0:b0a79a3a9da8 300 //***************************************************
nerit 0:b0a79a3a9da8 301 //***************************************************
nerit 0:b0a79a3a9da8 302 while (1){
nerit 0:b0a79a3a9da8 303 SwitchOnOff();
nerit 0:b0a79a3a9da8 304 }
nerit 0:b0a79a3a9da8 305 }