Francesco Pistone / Mbed 2 deprecated Viaro_SpandiConcime_V3_inizio

copia12092018

Dependencies:   mbed

main.cpp@4:1643fea75703, 2018-06-29 (annotated)

Committer:
viaromassimo
Date:
Fri Jun 29 13:07:46 2018 +0000
Revision:
4:1643fea75703
Parent:
3:1cd8d048cd4a
Child:
5:72bdc69d610d
V3;; rewritten.

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(){
viaromassimo 4:1643fea75703 149 pc.printf("\nMotorSpeedCorrected %f",MotorSpeedCorrected);
viaromassimo 4:1643fea75703 150 pc.printf(" - WantedMotorSpeed %d",WantedMotorSpeed);
viaromassimo 4:1643fea75703 151 pc.printf(" - ReadMotorSpeed %d",ReadMotorSpeed);
viaromassimo 4:1643fea75703 152 pc.printf(" - ElapsedW %d",ElapsedW);
viaromassimo 4:1643fea75703 153 pc.printf(" - TempReadW %d",TempReadW);
viaromassimo 4:1643fea75703 154 pc.printf(" - ElapsedM %d",ElapsedM);
viaromassimo 4:1643fea75703 155 pc.printf(" - TempReadM %d",TempReadM);
viaromassimo 4:1643fea75703 156 pc.printf(" - Plus %d",PercentPlus);
viaromassimo 4:1643fea75703 157 pc.printf(" - Minus %d",PercentMinus);
viaromassimo 4:1643fea75703 158 pc.printf(" - Security %d",SecurityStop);
nerit 0:b0a79a3a9da8 159 }
nerit 0:b0a79a3a9da8 160
nerit 0:b0a79a3a9da8 161 //***************************************************
nerit 0:b0a79a3a9da8 162 void CheckMotorCorrection(){
viaromassimo 4:1643fea75703 163 if(MotorSensorError==0 && SecurityStop==0){
viaromassimo 4:1643fea75703 164 CheckPercentSwitch();
viaromassimo 4:1643fea75703 165 WantedMotorSpeed=(float)((60000000/(ElapsedW*TrigsPerWheelRevolution))*WheelToMotorRatio*Percent); // =(( WHEEL_REVOLUTIONS/MINUTES )* RATIO CHANGE * +/- % ); rev/min
viaromassimo 4:1643fea75703 166 if(WantedMotorSpeed<MinMotorSpeed){
viaromassimo 4:1643fea75703 167 WantedMotorSpeed=0;
viaromassimo 4:1643fea75703 168 }
viaromassimo 4:1643fea75703 169
viaromassimo 4:1643fea75703 170 if(MachineType=="electric"){
viaromassimo 4:1643fea75703 171 motorPID.setSetPoint(WantedMotorSpeed);
viaromassimo 4:1643fea75703 172 motorPID.setProcessValue(ReadMotorSpeed);
viaromassimo 4:1643fea75703 173 MotorSpeedCorrected=motorPID.compute();
viaromassimo 4:1643fea75703 174 if(WantedMotorSpeed!=0 && uSMotor.read_us()>1000000){
viaromassimo 4:1643fea75703 175 MotorSensorSecurityCheck=MotorSensorSecurityCheck++;
viaromassimo 4:1643fea75703 176 if(MotorSensorSecurityCheck>50){
viaromassimo 4:1643fea75703 177 MotorSensorError=1; // OFF and ON to reset
viaromassimo 4:1643fea75703 178 }
viaromassimo 4:1643fea75703 179 }else{
viaromassimo 4:1643fea75703 180 MotorSensorSecurityCheck=0;
viaromassimo 4:1643fea75703 181 }
viaromassimo 4:1643fea75703 182 }
viaromassimo 4:1643fea75703 183
viaromassimo 4:1643fea75703 184 if(MachineType=="idraulic"){
viaromassimo 4:1643fea75703 185 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 186 }
viaromassimo 4:1643fea75703 187
viaromassimo 4:1643fea75703 188 MotorPwmPin.write(MotorSpeedCorrected);
viaromassimo 4:1643fea75703 189 DC_brake=0;
viaromassimo 4:1643fea75703 190 DC_prepare();
viaromassimo 4:1643fea75703 191
viaromassimo 4:1643fea75703 192 writelog();
viaromassimo 4:1643fea75703 193
viaromassimo 4:1643fea75703 194 }else{
viaromassimo 4:1643fea75703 195 OffTheMotor();
nerit 0:b0a79a3a9da8 196 }
nerit 0:b0a79a3a9da8 197 }
nerit 0:b0a79a3a9da8 198
nerit 0:b0a79a3a9da8 199 //***************************************************
nerit 0:b0a79a3a9da8 200 void CheckTimeElapsedFromLastTrig(){ //check time Elapsed from last wheel trig
viaromassimo 4:1643fea75703 201 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 202 OffTheMotor();
viaromassimo 4:1643fea75703 203 }else{
viaromassimo 4:1643fea75703 204 SecurityStop=0;
nerit 0:b0a79a3a9da8 205 }
nerit 0:b0a79a3a9da8 206 }
nerit 0:b0a79a3a9da8 207
nerit 0:b0a79a3a9da8 208 //***************************************************
nerit 0:b0a79a3a9da8 209 void SetMotorSpeed(){
viaromassimo 4:1643fea75703 210 CheckTimeElapsedFromLastTrig();
viaromassimo 4:1643fea75703 211
viaromassimo 4:1643fea75703 212 if(WheelData==1){
viaromassimo 4:1643fea75703 213 ElapsedW=0;
viaromassimo 4:1643fea75703 214 a=0;
viaromassimo 4:1643fea75703 215 for(i=0; i<(ReadWheelArray.size()-1); i++){
viaromassimo 4:1643fea75703 216 if(ReadWheelArray[i]!=1 && ReadWheelArray[i]!=0){ //avoid reading of 1 or 0 values in the array
viaromassimo 4:1643fea75703 217 a++; //number of all the valid readings
viaromassimo 4:1643fea75703 218 ElapsedW=ElapsedW+ReadWheelArray[i]; //all the valid readings are summed
viaromassimo 4:1643fea75703 219 }
viaromassimo 4:1643fea75703 220 }
viaromassimo 4:1643fea75703 221 ElapsedW=ElapsedW/a; //calculation of avarage of the valid readings
viaromassimo 4:1643fea75703 222 WheelData=0;
viaromassimo 3:1cd8d048cd4a 223 }
viaromassimo 4:1643fea75703 224
viaromassimo 4:1643fea75703 225 if(MotorData==1){
viaromassimo 4:1643fea75703 226 ElapsedM=0;
viaromassimo 4:1643fea75703 227 a=0;
viaromassimo 4:1643fea75703 228 for(i=0; i<(ReadMotorArray.size()-1); i++){
viaromassimo 4:1643fea75703 229 if(ReadMotorArray[i]!=1 && ReadMotorArray[i]!=0){ //avoid reading of 1 or 0 values in the array
viaromassimo 4:1643fea75703 230 a++; //number of all the valid readings
viaromassimo 4:1643fea75703 231 ElapsedM=ElapsedM+ReadMotorArray[i]; //all the valid readings are summed
viaromassimo 4:1643fea75703 232 }
viaromassimo 4:1643fea75703 233 }
viaromassimo 4:1643fea75703 234 ElapsedM=ElapsedM/a;
viaromassimo 4:1643fea75703 235 MotorData=0;
viaromassimo 4:1643fea75703 236 }
viaromassimo 4:1643fea75703 237
viaromassimo 4:1643fea75703 238 if(ElapsedM==0){
viaromassimo 4:1643fea75703 239 ReadMotorSpeed=0;
viaromassimo 3:1cd8d048cd4a 240 }else{
viaromassimo 4:1643fea75703 241 ReadMotorSpeed=float(60000000/(ElapsedM*4.0f)); // rev/min
viaromassimo 3:1cd8d048cd4a 242 }
nerit 0:b0a79a3a9da8 243 }
nerit 0:b0a79a3a9da8 244
nerit 0:b0a79a3a9da8 245 //***************************************************
nerit 0:b0a79a3a9da8 246 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 247 // pc.printf("\nTIME %d ",uSCycle.read_us());
viaromassimo 3:1cd8d048cd4a 248 // uSCycle.reset();
viaromassimo 4:1643fea75703 249 if(OnOffPin==0){
viaromassimo 4:1643fea75703 250 if(ChangeStatusOfOnOffPinToHigh==1){
viaromassimo 4:1643fea75703 251 ChangeStatusOfOnOffPinToHigh=0;
viaromassimo 4:1643fea75703 252 vibOnOffSwitch.start();
viaromassimo 4:1643fea75703 253 }else if(vibOnOffSwitch.read_ms()>VibrationTimer){
viaromassimo 4:1643fea75703 254 vibOnOffSwitch.stop();
viaromassimo 4:1643fea75703 255 if(ChangeFromOffToOn==1){
viaromassimo 4:1643fea75703 256 ChangeFromOffToOn=0;
viaromassimo 4:1643fea75703 257 }
viaromassimo 4:1643fea75703 258 DC_brake=0;
viaromassimo 4:1643fea75703 259 DC_prepare();
viaromassimo 4:1643fea75703 260 SetMotorSpeed();
viaromassimo 4:1643fea75703 261 }
viaromassimo 4:1643fea75703 262 }else{
viaromassimo 4:1643fea75703 263 ChangeStatusOfOnOffPinToHigh=1;
viaromassimo 4:1643fea75703 264 vibOnOffSwitch.stop();
viaromassimo 4:1643fea75703 265 vibOnOffSwitch.reset();
viaromassimo 4:1643fea75703 266 ChangeFromOffToOn=1;
nerit 0:b0a79a3a9da8 267 MotorSensorError=0;
viaromassimo 4:1643fea75703 268 OffTheMotor();
nerit 0:b0a79a3a9da8 269 }
nerit 0:b0a79a3a9da8 270 }
nerit 0:b0a79a3a9da8 271
nerit 0:b0a79a3a9da8 272 //***************************************************
nerit 0:b0a79a3a9da8 273 //***************************************************
nerit 0:b0a79a3a9da8 274 //***************************************************
viaromassimo 4:1643fea75703 275 int main(){
viaromassimo 4:1643fea75703 276 wait(1);
viaromassimo 4:1643fea75703 277 uSMotor.start();
viaromassimo 4:1643fea75703 278 uSWheel.start();
viaromassimo 4:1643fea75703 279 MotorPwmPin.period_us(2000); // freq 1khz
viaromassimo 4:1643fea75703 280 MotorPwmPin.write(0.0f); // duty cycle=off
viaromassimo 4:1643fea75703 281 OffTheMotor();
nerit 0:b0a79a3a9da8 282 MSToMotorNewCheck.attach(&CheckMotorCorrection, 0.1); //check motor correction every xx seconds
viaromassimo 4:1643fea75703 283 ReadWheelArray.clear();
viaromassimo 4:1643fea75703 284 ReadMotorArray.clear();
viaromassimo 4:1643fea75703 285 pc.printf("\n\n Hello World\n\n");
nerit 0:b0a79a3a9da8 286
viaromassimo 4:1643fea75703 287 WheelSensorPin.rise(&ReadWheelInterrupt); //interrupt for wheel sensor, trigs when changes from 0 to 1
viaromassimo 4:1643fea75703 288 MotorSensorPin.rise(&ReadMotorInterrupt); //interrupt for motor sensor, trigs when changes from 0 to 1
viaromassimo 4:1643fea75703 289
viaromassimo 4:1643fea75703 290 motorPID.setInputLimits(0, 10000); //70*secondosinaminute*trigsperwheelrevolutions
viaromassimo 4:1643fea75703 291 motorPID.setOutputLimits(0.0f, 1.0f);
viaromassimo 4:1643fea75703 292 motorPID.setMode(1);
viaromassimo 4:1643fea75703 293 motorPID.setSetPoint(1000);
nerit 0:b0a79a3a9da8 294
nerit 0:b0a79a3a9da8 295 //***************************************************
nerit 0:b0a79a3a9da8 296 //***************************************************
nerit 0:b0a79a3a9da8 297 //***************************************************
viaromassimo 4:1643fea75703 298 while(1){
viaromassimo 4:1643fea75703 299 SwitchOnOff();
viaromassimo 4:1643fea75703 300 }
nerit 0:b0a79a3a9da8 301 }