Arduino - Motor PID Speed Control

1void loop() {
2   if (stringComplete) {
3     // clear the string   when COM receiving is completed
4     mySt = "";  //note: in code below, mySt   will not become blank, mySt is blank until '\
5' is received
6     stringComplete   = false;
7   }
8  //receive command from Visual Studio
9   if (mySt.substring(0,8)   == "vs_start"){
10     digitalWrite(pin_fwd,1);      //run motor run forward
11      digitalWrite(pin_bwd,0);
12     motor_start = true;
13   }
14   if (mySt.substring(0,7)   == "vs_stop"){
15     digitalWrite(pin_fwd,0);
16     digitalWrite(pin_bwd,0);       //stop motor
17     motor_start = false;
18   }
19   if (mySt.substring(0,12)   == "vs_set_speed"){
20     set_speed = mySt.substring(12,mySt.length()).toFloat();   //get string after set_speed
21   }
22   if (mySt.substring(0,5) == "vs_kp"){
23      kp = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kp
24    }
25   if (mySt.substring(0,5) == "vs_ki"){
26     ki = mySt.substring(5,mySt.length()).toFloat();   //get string after vs_ki
27   }
28   if (mySt.substring(0,5) == "vs_kd"){
29      kd = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kd
30    } 
31 }
32void detect_a() {
33   encoder+=1; //increasing encoder at new pulse
34    m_direction = digitalRead(pin_b); //read direction of motor
35 }
36 ISR(TIMER1_OVF_vect)         // interrupt service routine - tick every 0.1sec
37 {
38   TCNT1 = timer1_counter;    // set timer
39   pv_speed = 60.0*(encoder/200.0)/0.1;  //calculate motor speed,   unit is rpm
40   encoder=0;
41   //print out speed
42   if (Serial.available()   <= 0) {
43     Serial.print("speed");
44     Serial.println(pv_speed);         //Print   speed (rpm) value to Visual Studio
45     }
46  //PID program
47   if (motor_start){
48      e_speed = set_speed - pv_speed;
49     pwm_pulse = e_speed*kp + e_speed_sum*ki   + (e_speed - e_speed_pre)*kd;
50     e_speed_pre = e_speed;  //save last (previous)   error
51     e_speed_sum += e_speed; //sum of error
52     if (e_speed_sum >4000)   e_speed_sum = 4000;
53     if (e_speed_sum <-4000) e_speed_sum = -4000;
54   }
55    else{
56     e_speed = 0;
57     e_speed_pre = 0;
58     e_speed_sum = 0;
59      pwm_pulse = 0;
60   }
61  //update new speed
62   if (pwm_pulse <255 & pwm_pulse   >0){
63     analogWrite(pin_pwm,pwm_pulse);  //set motor speed 
64   }
65   else{
66      if (pwm_pulse>255){
67       analogWrite(pin_pwm,255);
68     }
69     else{
70        analogWrite(pin_pwm,0);
71     }
72   }
73}

1void loop() {
2   if (stringComplete) {
3     // clear the string  when COM receiving is completed
4     mySt = "";  //note: in code below, mySt  will not become blank, mySt is blank until '\n' is received
5     stringComplete  = false;
6   }
7  //receive command from Visual Studio
8   if (mySt.substring(0,8)  == "vs_start"){
9     digitalWrite(pin_fwd,1);      //run motor run forward
10     digitalWrite(pin_bwd,0);
11     motor_start = true;
12   }
13   if (mySt.substring(0,7)  == "vs_stop"){
14     digitalWrite(pin_fwd,0);
15     digitalWrite(pin_bwd,0);      //stop motor
16     motor_start = false;
17   }
18   if (mySt.substring(0,12)  == "vs_set_speed"){
19     set_speed = mySt.substring(12,mySt.length()).toFloat();  //get string after set_speed
20   }
21   if (mySt.substring(0,5) == "vs_kp"){
22     kp = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kp
23   }
24   if (mySt.substring(0,5) == "vs_ki"){
25     ki = mySt.substring(5,mySt.length()).toFloat();  //get string after vs_ki
26   }
27   if (mySt.substring(0,5) == "vs_kd"){
28     kd = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kd
29   } 
30 }
31void detect_a() {
32   encoder+=1; //increasing encoder at new pulse
33   m_direction = digitalRead(pin_b); //read direction of motor
34 }
35 ISR(TIMER1_OVF_vect)        // interrupt service routine - tick every 0.1sec
36 {
37   TCNT1 = timer1_counter;   // set timer
38   pv_speed = 60.0*(encoder/200.0)/0.1;  //calculate motor speed,  unit is rpm
39   encoder=0;
40   //print out speed
41   if (Serial.available()  <= 0) {
42     Serial.print("speed");
43     Serial.println(pv_speed);         //Print  speed (rpm) value to Visual Studio
44     }
45  //PID program
46   if (motor_start){
47     e_speed = set_speed - pv_speed;
48     pwm_pulse = e_speed*kp + e_speed_sum*ki  + (e_speed - e_speed_pre)*kd;
49     e_speed_pre = e_speed;  //save last (previous)  error
50     e_speed_sum += e_speed; //sum of error
51     if (e_speed_sum >4000)  e_speed_sum = 4000;
52     if (e_speed_sum <-4000) e_speed_sum = -4000;
53   }
54   else{
55     e_speed = 0;
56     e_speed_pre = 0;
57     e_speed_sum = 0;
58     pwm_pulse = 0;
59   }
60  //update new speed
61   if (pwm_pulse <255 & pwm_pulse  >0){
62     analogWrite(pin_pwm,pwm_pulse);  //set motor speed 
63   }
64   else{
65     if (pwm_pulse>255){
66       analogWrite(pin_pwm,255);
67     }
68     else{
69       analogWrite(pin_pwm,0);
70     }
71   }
72}

1#pragma endregion
2 private: System::Void Form1_Load(System::Object^  sender, System::EventArgs^  e) {
3 serialPort1->Open();
4 timer1->Start();
5  mStr = "0";
6 i=300;
7 }
8 private: System::Void button1_Click(System::Object^  sender, System::EventArgs^  e) {
9 serialPort1->WriteLine("vs_set_speed"+textBox1->Text);  //send set_speed to Arduino
10 serialPort1->WriteLine("vs_kp"+textBox2->Text);  //send kP to Arduino
11 serialPort1->WriteLine("vs_ki"+textBox3->Text); //send  kI to Arduino
12 serialPort1->WriteLine("vs_kd"+textBox4->Text); //send kD to  Arduino
13 }
14 private: System::Void timer1_Tick(System::Object^  sender, System::EventArgs^  e) {
15 String^ length;
16 length=mStr->Length.ToString();
17 if(mStr->Substring(0,5)=="speed"){
18  speed=mStr->Substring(5,System::Convert::ToInt32(length)-6);
19 label1->Text=speed;
20  //print motor speed into Chart
21 this->chart1->Series["Series1"]->Points->AddXY(i,System::Convert::ToDouble(speed));
22  i++;
23 this->chart1->ChartAreas["ChartArea1"]->AxisX->Minimum=i-300; //shift  x-axis
24 }
25 }
26 private: System::Void serialPort1_DataReceived(System::Object^  sender, System::IO::Ports::SerialDataReceivedEventArgs^  e) {
27 mStr=serialPort1->ReadLine();
28  }
29 private: System::Void button2_Click(System::Object^  sender, System::EventArgs^  e) {
30 serialPort1->WriteLine("vs_start"); //start motor
31 }
32 private:  System::Void button3_Click(System::Object^  sender, System::EventArgs^  e) {
33  serialPort1->WriteLine("vs_stop");  //stop motor
34 }