SHARE
TWEET

misan

a guest Jan 20th, 2016 31 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2.  * Miguel Sanchez 2106
  3.    Mauro Manco 2016 Porting on ESP8266
  4.    
  5.    This program uses an Arduino Pro Micro variant for a closed-loop control of a DC-motor.
  6.    Motor motion is detected by a quadrature encoder.
  7.    Two inputs named STEP and DIR allow changing the target position.
  8.    Serial port prints current position and target position every second.
  9.    Serial input can be used to feed a new location for the servo (no CR LF).
  10.    
  11.    Pins used:
  12.    Digital inputs 2 & 3 are connected to the two encoder signals (AB).
  13.    Digital input 0 is the STEP input.
  14.    Analog input A0 is the DIR input.
  15.    Digital outputs 6 & 7 control the direction outputs for the motor (I am using half TB6612FNG here).
  16.    Digital output 9 is PWM motor control
  17.    Please note PID gains kp, ki, kd need to be tuned to each different setup.
  18. */
  19.  
  20. #include <EEPROM.h>
  21. #include <PID_v1.h>
  22. #include <ESP8266WiFi.h>
  23.  
  24. const char* ssid = "*****";
  25. const char* password = "******";
  26.  
  27. // Create an instance of the server
  28. // specify the port to listen on as an argument
  29. WiFiServer server(23);
  30. WiFiClient client;
  31.  
  32. const int encoder0PinA = 13;
  33. const int encoder0PinB = 12;
  34. const int Step = 14;
  35. const int M1=6;
  36. const int M2=7;
  37. const int DIR=4;
  38. const int PWM_MOT=15;
  39.  
  40. byte pos[1000]; int p=0;
  41. double kp=3,ki=0,kd=0.0;
  42. double input=0, output=0, setpoint=0;
  43. PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT);
  44. volatile long encoder0Pos = 0;
  45. boolean auto1=false, auto2=false,counting=false;
  46. long previousMillis = 0;        // will store last time LED was updated
  47.  
  48. long target1=0;  // destination location at any moment
  49.  
  50. //for motor control ramps 1.4
  51. bool newStep = false;
  52. bool oldStep = false;
  53. bool dir = false;
  54. byte skip=0;
  55.  
  56. void toggle() {
  57.   static int state = 0;
  58.   state = !state;
  59.   digitalWrite(BUILTIN_LED, state);
  60. }
  61.  
  62.  
  63.  
  64.   void pwmOut(int out) {
  65.    if(out>0) { digitalWrite(M1,0); digitalWrite(M2,1); }
  66.    else      { digitalWrite(M1,1); digitalWrite(M2,0); }
  67.    analogWrite(PWM_MOT,abs(out));
  68.    //PWM = out;
  69.   }
  70.  
  71. const int QEM [16] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0};               // Quadrature Encoder Matrix
  72. static unsigned char New, Old;
  73.  
  74.  
  75. void encoderInt() { // handle pin change interrupt for D2
  76.   Old = New;
  77.   //New = PIND & 3; //(PINB & 1 )+ ((PIND & 4) >> 1); //   Mauro Manco
  78.   New = digitalRead(encoder0PinA)*2 + digitalRead(encoder0PinB);
  79.   encoder0Pos+= QEM [Old * 4 + New];
  80. }
  81.  
  82.  
  83. void countStep(){ if (digitalRead(DIR)== HIGH) target1--;else target1++;
  84. } // pin A0 represents direction == PF7 en Pro Micro
  85.  
  86.  
  87.  
  88. void printPos() {
  89.   client.print(F("Position=")); client.print(encoder0Pos); client.print(F(" PID_output=")); client.print(output); client.print(F(" Target=")); client.println(setpoint);
  90. }
  91.  
  92.  
  93.  
  94. void help() {
  95.  client.println(F("\nPID DC motor controller and stepper interface emulator"));
  96.  client.println(F("by misan - porting cured by Exilaus"));
  97.  client.println(F("Available serial commands: (lines end with CRLF or LF)"));
  98.  client.println(F("P123.34 sets proportional term to 123.34"));
  99.  client.println(F("I123.34 sets integral term to 123.34"));
  100.  client.println(F("D123.34 sets derivative term to 123.34"));
  101.  client.println(F("? prints out current encoder, output and setpoint values"));
  102.  client.println(F("X123 sets the target destination for the motor to 123 encoder pulses"));
  103.  client.println(F("T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that"));
  104.  client.println(F("Q will print out the current values of P, I and D parameters"));
  105.  client.println(F("W will store current values of P, I and D parameters into EEPROM"));
  106.  client.println(F("H will print this help message again"));
  107.  client.println(F("A will toggle on/off showing regulator status every second\n"));
  108. }
  109.  
  110.  
  111. void eeput(double value, int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-(
  112.   char * addr = (char * ) &value;
  113.   for(int i=dir; i<dir+4; i++)  EEPROM.write(i,addr[i-dir]);
  114. }
  115.  
  116.  
  117. void writetoEEPROM() { // keep PID set values in EEPROM so they are kept when arduino goes off
  118.   eeput(kp,0);
  119.   eeput(ki,4);
  120.   eeput(kd,8);
  121.   double cks=0;
  122.   for(int i=0; i<12; i++) cks+=EEPROM.read(i);
  123.   eeput(cks,12);
  124.   client.println("\nPID values stored to EEPROM");
  125.   //Serial.println(cks);
  126. }
  127.  
  128.  
  129. double eeget(int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-(
  130.   double value;
  131.   char * addr = (char * ) &value;
  132.   for(int i=dir; i<dir+4; i++) addr[i-dir]=EEPROM.read(i);
  133.   return value;
  134. }
  135.  
  136. void recoverPIDfromEEPROM() {
  137.   double cks=0;
  138.   double cksEE;
  139.   for(int i=0; i<12; i++) cks+=EEPROM.read(i);
  140.   cksEE=eeget(12);
  141.   //Serial.println(cks);
  142.   if(cks==cksEE) {
  143.     client.println(F("*** Found PID values on EEPROM"));
  144.     kp=eeget(0);
  145.     ki=eeget(4);
  146.     kd=eeget(8);
  147.     myPID.SetTunings(kp,ki,kd);
  148.   }
  149.   else client.println(F("*** Bad checksum"));
  150. }
  151.  
  152.  
  153. void eedump() {
  154.  for(int i=0; i<16; i++) { client.print(EEPROM.read(i),HEX); client.print(" "); }client.println();
  155. }
  156.  
  157.  
  158. void setup() {
  159.   Serial.begin (115200);
  160.   pinMode(BUILTIN_LED, OUTPUT);
  161.   pinMode(encoder0PinA, INPUT);
  162.   pinMode(encoder0PinB, INPUT);
  163.   pinMode(Step, INPUT);
  164.   pinMode(PWM_MOT, OUTPUT);
  165.   attachInterrupt(encoder0PinA, encoderInt, CHANGE);
  166.   attachInterrupt(encoder0PinB, encoderInt, CHANGE);
  167.   attachInterrupt(Step, countStep, RISING);
  168.   toggle();
  169.   help();
  170.   recoverPIDfromEEPROM();
  171.   //Setup the pid
  172.   myPID.SetMode(AUTOMATIC);
  173.   myPID.SetSampleTime(1);
  174.   myPID.SetOutputLimits(-255,255);
  175.  
  176.     // Connect to WiFi network
  177.   Serial.println();
  178.   Serial.println();
  179.   Serial.print("Connecting to ");
  180.   Serial.println(ssid);
  181.  
  182.   WiFi.begin(ssid, password);
  183.  
  184.   while (WiFi.status() != WL_CONNECTED) {
  185.     delay(500);
  186.     Serial.print(".");
  187.   }
  188.   Serial.println("");
  189.   Serial.println("WiFi connected");
  190.  
  191.   // Start the server
  192.   server.begin();
  193.   Serial.println("Server started");
  194.  
  195.   // Print the IP address
  196.   Serial.println(WiFi.localIP());
  197. }
  198.  
  199. void process_line() {
  200.  char cmd = client.read();
  201.  if(cmd>'Z') cmd-=32;
  202.  switch(cmd) {
  203.   case 'P': kp=client.parseFloat(); myPID.SetTunings(kp,ki,kd); break;
  204.   case 'D': kd=client.parseFloat(); myPID.SetTunings(kp,ki,kd); break;
  205.   case 'I': ki=client.parseFloat(); myPID.SetTunings(kp,ki,kd); break;
  206.   case '?': printPos(); break;
  207.   case 'X': target1=client.parseInt(); counting=true; for(int i=0; i<p; i++) pos[i]=0; p=0; break;
  208.   case 'T': auto1 = !auto1; break;
  209.   case 'A': auto2 = !auto2; break;
  210.   case 'Q': client.print("P="); client.print(kp); client.print(" I="); client.print(ki); client.print(" D="); client.println(kd); break;
  211.   case 'H': help(); break;
  212.   case 'W': writetoEEPROM(); break;
  213.   case 'K': eedump(); break;
  214.   case 'R': recoverPIDfromEEPROM() ; break;
  215.   case 'S': for(int i=0; i<p; i++) client.println(pos[i]); break;
  216.  }
  217. // while(Serial.read()!=10); // dump extra characters till LF is seen (you can use CRLF or just LF)
  218. }
  219.  
  220.  
  221.  
  222. void loop() {
  223.   while(!client) client = server.available();
  224.     input = encoder0Pos;
  225.     setpoint=target1;
  226.     myPID.Compute();
  227.     //if(Serial.available()) process_line();
  228.     if(client && client.available()) process_line();
  229.     pwmOut(output);
  230.     if(auto1) if(millis() % 3000 == 0) target1=random(2000); // that was for self test with no input from main controller
  231.     if(auto2) if(millis() % 1000 == 0) printPos();
  232.     if(counting && abs(input-target1)<15) counting=false;
  233.     if(counting &&  (skip++ % 5)==0 ) {pos[p]=encoder0Pos; if(p<999) p++; else counting=false;}
  234.  
  235.    }
RAW Paste Data
Pastebin PRO Summer Special!
Get 60% OFF on Pastebin PRO accounts!
Top