Diff: MotorDriver_ver4-1.cpp

Revision:

7:d71bef2faad7

Parent:

5:1155a15f978c

--- a/MotorDriver_ver4-1.cpp	Sun Aug 25 15:39:22 2019 +0000
+++ b/MotorDriver_ver4-1.cpp	Fri Mar 13 04:40:42 2020 +0000
@@ -1,9 +1,9 @@
 /************************************************
 MotorDriver ver4.1 farmware
-author : shundai miyawaki
+author : toshihiro
 2019/06/14 ~ 
-
-Copyright shundai miyawaki. All rights reserved.
+Twitter : @WaatH5
+Copyright tosihiro. All rights reserved.
 
 ************************************************/
 
@@ -37,13 +37,13 @@
 // ゲインはモータードライバー固有のパラメータなのでflashに保存する，
 // 初期値としてデフォルトの値をdefineで定義する
 // theta feedback PID gain
-#define theta_Kp 9.0f
-#define theta_Ki 0.0f
+#define theta_Kp 1.0f
+#define theta_Ki 0.05f
 #define theta_Kd 0.00001f
 
 // omega feedback PID gain
 #define omega_Kp 0.05f
-#define omega_Ki 0.05f
+#define omega_Ki 0.02f
 #define omega_Kd 0.0f
 
 
@@ -125,8 +125,8 @@
     phase : A, B, Z
     voltage : 10.8V - 20.0V
 */
-//#define ENCODER_TRD_S1000B
-#define ENCODER_E6A2_CW5C
+#define ENCODER_TRD_S1000B
+//#define ENCODER_E6A2_CW5C
 
 #if defined ENCODER_TRD_S1000B
 #define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
@@ -170,6 +170,7 @@
 InterruptIn userButton(PB_5);
 Ticker Encoder;
 Ticker SEND;
+Ticker DEMO;
 Timer ENCODER_timer;
 
 struct state
@@ -190,6 +191,7 @@
 
 void Velocity();
 void ButtonFunction();
+void demo();
 void SerialFunction();
 void CANread();
 void CANsend();
@@ -204,7 +206,7 @@
 void writeFlash(uint32_t address, uint8_t *data, uint32_t size);
 void eraseFlash(uint32_t address);
 
-float buf = 0.0f;
+//float buf = 0.0f;
 
 int main()
 {
@@ -322,6 +324,7 @@
 
     while(1)
     { 
+        /*
         pc.printf("%f \r\n", buf);
         
         if(buf > 0.0f)
@@ -335,7 +338,7 @@
             PWM1 = buf;
             PWM2 = 0.0f;
         }
-        
+        */
         //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
         
         //DBG("omegaPID.compute : %6.3f thetaPID.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", omegaPID.compute(), thetaPID.compute(), Motor.theta, Motor.omega);
@@ -390,31 +393,31 @@
         switch (data)
         {
             case 'p': 
-                //Motor.PID = THETA_FEEDBACK;
-                //Motor.ThetaTarget += 30.0f;
-                buf += 0.05f;
+                Motor.PID = THETA_FEEDBACK;
+                Motor.ThetaTarget += 30.0f;
+                //buf += 0.05f;
                 DBG("get 'p' \r\n");    
                 
                 break;
             
             case 'l':
-                //Motor.PID = THETA_FEEDBACK;
-                //Motor.ThetaTarget -= 30.0f;
-                buf -= 0.05f;
+                Motor.PID = THETA_FEEDBACK;
+                Motor.ThetaTarget -= 30.0f;
+                //buf -= 0.05f;
                 DBG("get 'l' \r\n");
                 
                 break;
                 
             case 'o': 
                 Motor.PID = OMEGA_FEEDBACK;  
-                Motor.OmegaTarget += 10.0f;
+                Motor.OmegaTarget += 30.0f;
                 DBG("get 'q' \r\n");    
                 
                 break;
             
             case 'k':
                 Motor.PID = OMEGA_FEEDBACK;
-                Motor.OmegaTarget -= 10.0f;
+                Motor.OmegaTarget -= 30.0f;
                 DBG("get 'a' \r\n");
                 
                 break;
@@ -447,7 +450,7 @@
         }
     }
 }
-
+/*
 void ButtonFunction()
 {
     //serial menu printf
@@ -479,7 +482,7 @@
     //ボタン情報を割込みで入力させる関数を用意してボタン情報を入力する．
     
     //モータが駆動し始める電流を測定する．（パルスが増えるまでＰＷＭを微小増加させる）
-    /*
+    
     Motor.OmegaTarget = 60.0f; //[rpm]
     
     while(1)
@@ -488,7 +491,91 @@
         PWM1 = 0.0f;
         PWM2 = 0.0f;
     }
-    */
+    
+}
+*/
+
+
+void ButtonFunction()
+{
+    Motor.OmegaTarget = 0.0f; //motor stop
+    
+    DEMO.attach(demo, 0.2);
+     
+    DBG("setup open \r\n");
+    DBG("1:change the CAN device ID \r\n");
+    DBG("2:change the CAN device ID \r\n");
+    
+    //CANtest();  //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
+    
+    //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
+    
+}
+
+void demo()
+{
+    static int phase;
+        
+    switch (phase%8)
+    {
+        case 0:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = 30.0f;
+        
+            break;
+            
+        case 1:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = 60.0f;
+            
+            break;
+        
+        case 2:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = 30.0f;
+        
+            break;
+            
+        case 3:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = 0.0f;
+        
+            break;
+            
+        case 4:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = -30.0f;
+            
+            break;
+        
+        case 5:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = -60.0f;
+        
+            break;
+            
+        case 6:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = -30.0f;
+        
+            break;
+            
+        case 7:
+        
+            Motor.PID = OMEGA_FEEDBACK;  
+            Motor.OmegaTarget = 0.0f;
+            
+            break;    
+    }
+    
+    phase++;
 }
 
 void CANread()
@@ -759,34 +846,3 @@
     //old_direction = direction;
     //old_target = target;
 }
-
-void eraseFlash(uint32_t address)
-{
-    FLASH_EraseInitTypeDef erase;
-    erase.TypeErase = FLASH_TYPEERASE_PAGES;    /* ページの消去を選択 */
-    erase.PageAddress = address;                /* ページの先頭アドレスを指定 */
-    erase.NbPages = 1;      /* 消すページの数．今回は1つだけ */
-
-    uint32_t pageError = 0;
-
-    HAL_FLASHEx_Erase(&erase, &pageError);  /* HAL_FLASHExの関数で消去 */
-}
-
-void writeFlash(uint32_t address, uint8_t *data, uint32_t size)
-{
-    uint32_t *dataWord = (uint32_t*)data;   /* 書き込むデータへのポインタ(4Byteごと) */
-    uint32_t sizeWord = (size+3)/4;    /* データのサイズ(4Byteで1) */
-    
-    HAL_FLASH_Unlock();        /* フラッシュをアンロック */
-    eraseFlash(address);            /* 指定したアドレスのページを消去 */
-    do {
-        /* 4Byte(Word)ずつフラッシュに書き込む */
-        HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, *dataWord);
-    } while (address+=4, ++dataWord, --sizeWord);
-     HAL_FLASH_Lock();        /* フラッシュをロック */
-}
-
-void loadFlash(uint32_t address, uint8_t *data, uint32_t size)
-{
-    memcpy(data, (uint8_t*)address, size);
-}
\ No newline at end of file