User Tools

Site Tools


pid_control_-_pneumatics_inverted_pendulum

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
pid_control_-_pneumatics_inverted_pendulum [2017/07/10 17:18]
hyunheelee [Matlab Code]
pid_control_-_pneumatics_inverted_pendulum [2017/07/10 17:18] (current)
hyunheelee [Arduino code for PID]
Line 141: Line 141:
 ===== Arduino code for PID ===== ===== Arduino code for PID =====
 <code java> <code java>
-#define MOTOR OUT_A 
-#define PNDLM OUT_C 
-#define FULL_SPEED 100 
-#define D2R 0.0175 // converts degree to radian 
-#define R2D 57.2958 // converts radian to degree 
- 
-long prev_tick = 0; 
-long prev_Mdeg = 0; 
-long prev_Pdeg = 0; 
-long Mdeg = 0; 
-long Pdeg = 0; 
-long M_diff = 0; 
-long P_diff = 0; 
- 
-float dt = 0.0; 
-float GTime = 0.0; 
-float Mrad = 0.0; 
-float Prad = 0.0; 
-float d_Mrad = 0.0; 
-float d_Prad = 0.0; 
-float d_Mdeg = 0.0; 
-float d_Pdeg = 0.0; 
-float M_errSum = 0.0; 
  
 char controlU = 0; char controlU = 0;
Line 191: Line 168:
    ​M_errSum = M_errSum + error - 1.0*(U - controlU);    ​M_errSum = M_errSum + error - 1.0*(U - controlU);
        
-   ​OnRev(MOTOR,​ controlU); 
-} 
- 
-sub MotorOpenLoop(float speed) 
-{ 
-   ​if(speed > FULL_SPEED) 
-   { 
-      controlU = FULL_SPEED; 
-   } 
-   else if(speed < -FULL_SPEED) 
-   { 
-      controlU = -FULL_SPEED;​ 
-   } 
-   else 
-   { 
-      controlU = speed; 
-   } 
- 
-   ​OnRev(MOTOR,​ controlU); 
-} 
- 
-task ReadEncoder() 
-{ 
-   const int w = 4; 
-   float M_mov[w] = {0.0,}; 
-   float P_mov[w] = {0.0,}; 
-   float sum_Mmov = 0.0; 
-   float sum_Pmov = 0.0; 
-   int cnt = 0; 
- 
-   ​while(true) 
-   { 
-      dt = (CurrentTick() - prev_tick)*0.001;​ 
-      Mdeg = MotorRotationCount(MOTOR);​ 
-      Pdeg = MotorRotationCount(PNDLM);​ 
-      M_diff = Mdeg - prev_Mdeg; 
-      P_diff = Pdeg - prev_Pdeg; 
-      prev_Mdeg = Mdeg; 
-      prev_Pdeg = Pdeg; 
- 
-      d_Mdeg = M_diff/dt; 
-      d_Pdeg = P_diff/dt; 
- 
-      Mrad = Mdeg*D2R; 
-      Prad = Pdeg*D2R; 
- 
-      d_Mrad = d_Mdeg*D2R; 
-      d_Prad = d_Pdeg*D2R; 
- 
-      prev_tick = CurrentTick();​ 
-      GTime += dt; 
- 
-      Wait(MS_20);​ 
-   } 
-} 
- 
-task BalanceControl() 
-{ 
-   // A physical motor rotation direction is reversed as a model'​s rotation direction. 
-   float Gm_p = -(-7.07106781265702); ​    //​Gain on Motor Position 
-   float Gp_p = -361.604865668371; ​       //Gain on Pendulum Position 
-   float Gm_v = -(-7.20613568524004); ​    //​Gain on Motor Velocity 
-   float Gp_v = -9.15404034187486; ​      //​Gain on Pendulum velocity 
- 
-   ​Wait(MS_10);​ 
-    
-   ​while(true) 
-   { 
-      if(flag_balanceControl == true) 
-      { 
-         ​MotorOpenLoop(Gp_p*Prad + Gp_v*d_Prad + Gm_p*Mrad + Gm_v*d_Mrad);​ 
-      } 
-      Wait(MS_20);​ 
-   } 
-} 
- 
-task Display() 
-{ 
-   ​while(true) 
-   { 
-      TextOut(0, LCD_LINE1, FormatNum("​Frq:​ %.1f Hz", 1/dt), DRAW_OPT_CLEAR_LINE);​ 
-      TextOut(0, LCD_LINE2, FormatNum("​P deg: %d", Pdeg), DRAW_OPT_CLEAR_LINE);​ 
-      TextOut(0, LCD_LINE3, FormatNum("​M deg: %d", Mdeg), DRAW_OPT_CLEAR_LINE);​ 
-      TextOut(0, LCD_LINE4, FormatNum("​Time:​ %.3f", GTime), DRAW_OPT_CLEAR_LINE);​ 
-      TextOut(0, LCD_LINE5, FormatNum("​Flag_B:​ %d", flag_balanceControl),​ DRAW_OPT_CLEAR_LINE);​ 
-      TextOut(0, LCD_LINE6, FormatNum("​Ctrl U: %d", controlU), DRAW_OPT_CLEAR_LINE);​ 
-      Wait(MS_200);​ 
-   } 
-} 
- 
-task WriteData() 
-{ 
-   short bytesWritten;​ 
-   ​string write; 
-   long cnt = 0; 
-    
-   ​CreateFile("​data_sp.txt",​ 51200, fileHandle);​ 
-    
-   ​while(true) 
-   { 
-      ++cnt; 
-      write = NumToStr(cnt);​ 
-      write = StrCat(write,​ " ", NumToStr(GTime),​ " ", NumToStr(Mrad),​ " ", NumToStr(d_Mrad),​ " ", NumToStr(Prad),​ " ", NumToStr(d_Prad));​ 
-      WriteLnString(fileHandle,​ write, bytesWritten);​ 
-      Wait(MS_5); 
-   } 
-} 
- 
-task main() 
-{ 
-   ​DeleteFile("​data_sp.txt"​);​ 
  
-   ​prev_tick = CurrentTick();​ 
-   ​Precedes(ReadEncoder,​ BalanceControl,​ Display); 
-   //​Precedes(ReadEncoder,​ Display); 
-   //​Precedes(ReadEncoder,​ WriteData, Display); 
-   //​Precedes(ReadEncoder,​ BalanceControl,​ WriteData, Display); 
-} 
 </​code>​ </​code>​
  
            
pid_control_-_pneumatics_inverted_pendulum.txt ยท Last modified: 2017/07/10 17:18 by hyunheelee