////////POUR ATMEGA1284P ATMEGA1284 but not ATMEGA32 //U=32V N=7400RPM,// U=12V=255 N=2775RPM,// U=11,76V=250 N=2720RPM //mechanical contant: 21ms #include #include #include #include #include SM GR7RUNSTOP(&etape10); SM Gr7Secu(&etape1); SM DisplayRPM(&etape100);//display position and speed SM DisplayCHA(&etape200); SM INTERRUPT_CHA(&etape400); LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE); // Set the LCD I2C address to 27 ////////////////////////////////////////////////////////////////////////// // macro for detection of rising edge and debouncing /*the state argument (which must be a variable) records the current and the last 7 reads by shifting one bit to the left at each read. If the value is 15(=0b00001111) we have one rising edge followed by 4 consecutive 1's. That would qualify as a debounced rising edge*/ #define DRE(signal, state) (state=(state<<1)|signal)==B00001111 // Rising state variables for each button byte REState; // macro for detection of falling edge and debouncing /*the state argument (which must be a variable) records the current and the last 7 reads by shifting one bit to the left at each read. If the value is 240(=0b11110000) we have one falling edge followed by 4 consecutive 0's. That would qualify as a debounced falling edge*/ #define DFE(signal, state) (state=(state<<1)|signal)==B11110000 // Falling state variables for each button byte FEState; ////////////////////////////////////////////////////////////////////////////// #define driverPOM 22 // POM- pin #define driverPOM2 23 // POM- pin int LIGHT = 3; // A3 LIGHT- pin #define CHA 10 #define CHB 11 #define DIR 2 //HIGH:CCW, LOW:CW #define RUN 9 #define STOP 8 int sens; volatile long master_count; //count CHA volatile long master_countdisp; volatile long step_count = 0; //count STEPS SENT volatile long master_count2; //count CHA volatile long master_countdisp2; volatile long step_count2 = 0; //count STEPS SENT unsigned long currentTime = 0; unsigned long previousTime = 0; unsigned long currentTime2 = 0; unsigned long previousTime2 = 0; unsigned long currentTime3 = 0; unsigned long previousTime3 = 0; float RPM; float RPM2 = 0; float RPM3; int CHAvalue; int CHBvalue; signed long TURNcount = 0; signed long TURNcount2 = 0; signed long positionDEG = 0; signed long positionDEG2 = 0; signed long MESpositionDEG2 = 0; signed long SPpositionDEG2 = 0; int potentiometre = A0; int PWMout = 15;//pwm output float SetPointDeg = 30; float SetPointTurn = 0; int Flag; int Flag2 = 0; volatile long tick_codeuse = 0; volatile float Kp, Ti, Td, T, ek, ek_1, ek_2, a, b, uk, uk_1; SoftwareSerial mySerial(0, 1); // RX, TX void setup() { pinMode (7, OUTPUT); pinMode (driverPOM, INPUT); pinMode (driverPOM2, INPUT); pinMode (RUN, INPUT); pinMode (STOP, INPUT); pinMode(DIR, OUTPUT); pinMode (LIGHT, OUTPUT); pinMode(14, OUTPUT); pinMode(CHA, INPUT);//quadrature coder pulses for SPEED pinMode(CHB, INPUT);//quadrature coder pulses for ROTATION SENS lcd.begin(20, 4); lcd.backlight();//NO USE lcd.clear(); // initialize the lcd lcd.setCursor ( 0, 0 ); // go to the next line lcd.print (" CODER EXAMPLE "); //sets the frequency for the specified pin TCCR2B = TCCR2B & 0b11111000 | 0x01;//0x01 1 31250 ,disturb pin 14,15 RESETdisp(); mySerial.begin(115200); cli();//stop interrupts for till we make the settings //set timer3 interrupt at 10Hz, no problem with timer2 TCCR3A = 0; TCCR3B = 0; // Set compare match register to desired timer count - 25ms 40Hz ((16000000/1024)/40 = 390.63) // 10Hz for 1500, signal a bit disturbed //50Hz for 313 //100HZ for 156 //200Hz for 78, sample period 5 ms //5HZ 3124 20HZ 781 OCR3A = 313; // Turn on CTC mode WGM32 set CS10 and CS12 bits for Timer3 prescaler of 1024 TCCR3B = bit (WGM32) | bit (CS32) | bit (CS30); TIMSK3 |= (1 << OCIE3A); sei(); //Enable back the interrupts Kp = 1; T = 0.020032; Ti = 0.07; Td = 0.01; a = Kp + (2 * Td / T); b = Kp + (T / Ti) + (Td / T); uk_1 = 0; ek_1 = 0; ek_2 = 0; } void loop() { EXEC (GR7RUNSTOP); EXEC (INTERRUPT_CHA); EXEC(DisplayCHA);// display STEP from PWM sent to HSS86, rev and position EXEC(DisplayRPM);//display position and speed EXEC(Gr7Secu); TURNcount = step_count / 500; //ONE TURN:500 PULSES positionDEG = 360 * (step_count - (TURNcount * 500)) / 500; } ISR(TIMER3_COMPA_vect) { //display on SerialPlot with no disturbs ///DISPLAYS SPEED MEASURMENT AND SET POINT ON SERIAL AND PID CALCULATION if (Flag == 1) { ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { TURNcount2 = step_count2 / 500; //ONE TURN:500 PULSES positionDEG2 = 360 * (step_count2 - (TURNcount2 * 500)) / 500; MESpositionDEG2 = positionDEG2 + (TURNcount2 * 360); SPpositionDEG2 = SetPointDeg + (SetPointTurn * 360); ek = (SPpositionDEG2) - (MESpositionDEG2); if (ek > 0) { digitalWrite(DIR, HIGH);//ccw uk = uk_1 + (Td / T) * ek_2 - (a * ek_1) + (b * ek); //if (uk < 0) uk = 0; if (uk > 245) uk = 245;// else analogWrite(PWMout, abs(uk)); uk_1 = uk; ek_1 = ek; ek_2 = ek_1; } if (ek <= 0) { digitalWrite(DIR, LOW);//cw uk = uk_1 + (Td / T) * ek_2 - (a * ek_1) + (b * ek); //if (uk < 0) uk = 0; if (abs(uk) > 245) uk = 245;//else analogWrite(PWMout, abs(uk)); uk_1 = uk; ek_1 = ek; ek_2 = ek_1; } mySerial.print(MESpositionDEG2); mySerial.print(","); mySerial.print(SPpositionDEG2); mySerial.print(","); mySerial.println(ek); } } else { //ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { digitalWrite(7, Flag2); Flag2 = !Flag2; master_countdisp2 = master_count2; TURNcount2 = step_count2 / 500; //ONE TURN:500 PULSES positionDEG2 = 360 * (step_count2 - (TURNcount2 * 500)) / 500; MESpositionDEG2 = positionDEG2 + (TURNcount2 * 360); SPpositionDEG2 = SetPointDeg - (SetPointTurn * 360); uk_1 = 0; ek_1 = 0; ek_2 = 0; analogWrite(PWMout, 0); mySerial.print(MESpositionDEG2); mySerial.print(","); mySerial.print(0); mySerial.print(","); mySerial.println(ek); } } //////////////////////////INTERRUPT ON CHA/////////////////////////////////////// State etape400() { attachInterrupt(0, Acount, RISING);//PUSLE count on CHA for speed calculation } //////////////////////////////RUN AND STOP/////////////////////////////////////////// State etape10() { RESETdisp(); digitalWrite(14, LOW); analogWrite(PWMout, 0); Flag = 0; if (DRE(digitalRead(RUN), REState)) GR7RUNSTOP.Set(etape11); } State etape11() { digitalWrite(14, HIGH); Flag = 1; if (DRE(digitalRead(STOP), REState)) GR7RUNSTOP.Set(etape10); } ///////////////////////////////////RESET ROTOR POSITION//////////////////////////////////// State etape1() { digitalWrite(LIGHT, 0); if (digitalRead(driverPOM) == 1) Gr7Secu.Set(etape2); } State etape2() { step_count2 = 0; ek = 0; uk = 0; uk_1 = 0; ek_1 = 0; ek_2 = 0; digitalWrite(LIGHT, 1); if (digitalRead(driverPOM2) == 1) Gr7Secu.Set(etape1); } ///////////////////////////////////////////////////////////////////// //___________________________DisplayRPM state diagram___________________ State etape100() {//display EA+ count every 1s, rpm on HMI currentTime = micros(); if ((currentTime - previousTime) > 1000000) { previousTime = currentTime; RPMdisp();//displays RPM ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {//avoid measurments disturb master_countdisp = master_count; RPM = 0.12 * master_countdisp; //ON CHA measurement (x60 /500), //500 pulse/tr master_count = 0; } currentTime = micros(); } } //______________DisplayCHA state diagram__________________________ State etape200() {//display pulses on turns currentTime2 = micros(); if ((currentTime2 - previousTime2) > 350000) {//less than 300000: STRANGE DISPLAY previousTime2 = currentTime2; CHAdisp(); TURNdisp(); POSITIONdisp(); currentTime2 = micros(); } } ///////////////////////COUNT AND SENS ON CHA PULSES/////////////////////////// void Acount() { if ((PIND & B00000100) && (PIND & B00001000)) {//READ PD2 AND PD3 quickly step_count-- ;//USED FOR SPEED AND TURN CALCULATION step_count2--; master_count-- ;//USED FOR DISPLAYING AND RESET EVERY 1S master_count2-- ; tick_codeuse--; } else { step_count++ ; step_count2++; master_count++ ; master_count2++ ; tick_codeuse++; } } //////////////////////////DISPLAY PULSES///////////////////////// void CHAdisp() {//display step counted lcd.setCursor ( 0, 2 ); // go to the next line lcd.print ("PLS on CHA:"); lcd.setCursor ( 19, 2 ); // go to the next line lcd.print (" "); if ((step_count < 10) && (step_count >= 0)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 18, 2 ); // go to the next line lcd.print (step_count); } if ((step_count < 0) && (step_count > -10)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 17, 2 ); // go to the next line lcd.print (step_count); } if ((step_count >= 10) && (step_count < 100)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 17, 2 ); // go to the next line lcd.print (step_count); } if ((step_count <= -10) && (step_count > -100)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 16, 2 ); // go to the next line lcd.print (step_count); } if ((step_count >= 100) && (step_count < 1000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 16, 2 ); // go to the next line lcd.print (step_count); } if ((step_count <= -100) && (step_count > -1000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 15, 2 ); // go to the next line lcd.print (step_count); } if ((step_count >= 1000) && (step_count < 10000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 15, 2 ); // go to the next line lcd.print (step_count); } if ((step_count <= -1000) && (step_count > -10000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 14, 2 ); // go to the next line lcd.print (step_count); } if ((step_count >= 10000) && (step_count < 100000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 14, 2 ); // go to the next line lcd.print (step_count); } if ((step_count <= -10000) && (step_count > -100000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 13, 2 ); // go to the next line lcd.print (step_count); } if ((step_count >= 100000) && (step_count < 1000000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 13, 2 ); // go to the next line lcd.print (step_count); } if ((step_count <= -100000) && (step_count > -1000000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 12, 2 ); // go to the next line lcd.print (step_count); } if ((step_count >= 1000000) && (step_count < 10000000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 12, 2 ); // go to the next line lcd.print (step_count); } if ((step_count <= -1000000) && (step_count > -10000000)) { lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (""); lcd.print (step_count); } } /////////////////////////DISPLAY RPM//////////////////////////////// void RPMdisp() {//display RPM calculated on pulse count lcd.setCursor ( 0, 1 ); // go to the next line lcd.print ("RPM on CHA:"); if ((RPM < 10) && (RPM >= 0)) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 15, 1 ); // go to the next line lcd.print (RPM); } if ((RPM < 0) && (RPM > -9)) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 14, 1 ); // go to the next line lcd.print (RPM); } if ((RPM > 9) && (RPM < 100)) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 14, 1 ); // go to the next line lcd.print (RPM); } if ((RPM < -9) && (RPM > -99)) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 13, 1 ); // go to the next line lcd.print (RPM); } if ((RPM > 99) && (RPM < 1000)) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 13, 1 ); // go to the next line lcd.print (RPM); } if ((RPM < -99) && (RPM > -999)) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 12, 1 ); // go to the next line lcd.print (RPM); } if (RPM > 999) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 12, 1 ); // go to the next line lcd.print (RPM); } if (RPM < -999) { lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (RPM); } } ////////////////////////////DISPLAY NUMBER OF TURNS///////////////////// void TURNdisp() {//display turns done lcd.setCursor ( 0, 3); // go to the next line lcd.print ("TRN:"); if ((TURNcount < 10) && (TURNcount >= 0)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 9, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount < 0) && (TURNcount > -9)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 8, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount > 9) && (TURNcount < 100)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 8, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount < -9) && (TURNcount > -99)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 7, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount > 99) && (TURNcount < 1000)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 7, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount < -99) && (TURNcount > -999)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 6, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount > 999) && (TURNcount < 10000)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 6, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount < -999) && (TURNcount > -9999)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 5, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount > 9999) && (TURNcount < 100000)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 5, 3 ); // go to the next line lcd.print (TURNcount); } if ((TURNcount < -9999) && (TURNcount > -99999)) { lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (TURNcount); } } //////////////////////////DISPLAY POSITION IN DEGREES//////////////// void POSITIONdisp() {//display the position remain in degrees lcd.setCursor ( 11, 3); // go to the next line lcd.print ("DEG:"); if ((positionDEG < 10) && (positionDEG >= 0)) { lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 18, 3 ); // go to the next line lcd.print (positionDEG); } if ((positionDEG < 0) && (positionDEG > -9)) { lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 17, 3 ); // go to the next line lcd.print (positionDEG); } if ((positionDEG > 9) && (positionDEG < 100)) { lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 17, 3 ); // go to the next line lcd.print (positionDEG); } if ((positionDEG < -9) && (positionDEG > -99)) { lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 16, 3 ); // go to the next line lcd.print (positionDEG); } if ((positionDEG > 99) && (positionDEG < 999)) { lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 16, 3 ); // go to the next line lcd.print (positionDEG); } if ((positionDEG < -99) && (positionDEG > -999)) { lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (positionDEG); } } ////////////////////RESET DISPLAY//////////////////// void RESETdisp() {//display the position remain in degrees step_count = 0; master_count = 0; master_countdisp = 0; RPM = 0; lcd.setCursor ( 0, 0 ); // go to the next line lcd.print (" CODER EXAMPLE "); lcd.setCursor ( 11, 1 ); // go to the next line lcd.print (" "); lcd.setCursor ( 15, 1 ); // go to the next line lcd.print ("0.00 "); lcd.setCursor ( 10, 2 ); // go to the next line lcd.print (":"); lcd.setCursor ( 11, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 18, 2 ); // go to the next line lcd.print ("0"); lcd.setCursor ( 19, 2 ); // go to the next line lcd.print (" "); lcd.setCursor ( 4, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 9, 3 ); // go to the next line lcd.print ("0"); lcd.setCursor ( 10, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 15, 3 ); // go to the next line lcd.print (" "); lcd.setCursor ( 18, 3 ); // go to the next line lcd.print ("0"); }