Massimo Viaro / Mbed 2 deprecated Viaro_SpandiConcime_V3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Viaro_SpandiConcime_V2b by Neri Tiziano

main.cpp@2:5111c6c0639e, 2018-06-15 (annotated)

Committer:
viaromassimo
Date:
Fri Jun 15 17:04:46 2018 +0000
Revision:
2:5111c6c0639e
Parent:
1:59c30e854dfb
Child:
3:1cd8d048cd4a
June, 15 2018 - 19:04

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