shundai miyawaki
モータードライバーver4.1 角度フィードバック（クーロン摩擦補償） 角速度フィードバック
Dependencies: mbed QEI PID DBG
MotorDriver_ver4-1.cpp@7:d71bef2faad7, 2020-03-13 (annotated)
- Committer:
- shundai
- Date:
- Fri Mar 13 04:40:42 2020 +0000
- Revision:
- 7:d71bef2faad7
- Parent:
- 5:1155a15f978c
NHK MD4.2
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| shundai | 4:40a764acc4ec | 1 | /************************************************ |
| shundai | 1:844acc585d3d | 2 | MotorDriver ver4.1 farmware |
| shundai | 7:d71bef2faad7 | 3 | author : toshihiro |
| shundai | 1:844acc585d3d | 4 | 2019/06/14 ~ |
| shundai | 7:d71bef2faad7 | 5 | Twitter : @WaatH5 |
| shundai | 7:d71bef2faad7 | 6 | Copyright tosihiro. All rights reserved. |
| shundai | 4:40a764acc4ec | 7 | |
| shundai | 4:40a764acc4ec | 8 | ************************************************/ |
| shundai | 1:844acc585d3d | 9 | |
| shundai | 5:1155a15f978c | 10 | /* |
| shundai | 5:1155a15f978c | 11 | |
| shundai | 5:1155a15f978c | 12 | // target board select checker of stm32f042k6t6 |
| shundai | 5:1155a15f978c | 13 | //未検証 |
| shundai | 5:1155a15f978c | 14 | #if !defined(TARGET_STM32F042K6) |
| shundai | 5:1155a15f978c | 15 | #error This code is only for NUCLEO-F042K6 or STM32F042K6T6. Reselect a Platform. |
| shundai | 5:1155a15f978c | 16 | #endif |
| shundai | 5:1155a15f978c | 17 | */ |
| shundai | 5:1155a15f978c | 18 | |
| shundai | 5:1155a15f978c | 19 | // target board select checker of stm32f303k8t6 |
| shundai | 5:1155a15f978c | 20 | //検証済 |
| shundai | 1:844acc585d3d | 21 | #if !defined(TARGET_STM32F303K8) |
| shundai | 1:844acc585d3d | 22 | #error This code is only for NUCLEO-F303K8 or STM32F303K8T6. Reselect a Platform. |
| shundai | 1:844acc585d3d | 23 | #endif |
| shundai | 1:844acc585d3d | 24 | |
| shundai | 5:1155a15f978c | 25 | // debug switch |
| shundai | 5:1155a15f978c | 26 | //#define DEBUG //debug ON |
| shundai | 5:1155a15f978c | 27 | #undef DEBUG //debug OFF |
| shundai | 1:844acc585d3d | 28 | |
| shundai | 5:1155a15f978c | 29 | // preproseccer |
| shundai | 1:844acc585d3d | 30 | #include "dbg.h" |
| shundai | 1:844acc585d3d | 31 | #include "mbed.h" |
| shundai | 5:1155a15f978c | 32 | #include "PID.h" |
| shundai | 5:1155a15f978c | 33 | #include "QEI.h" |
| shundai | 1:844acc585d3d | 34 | //#include <CANformat.h> |
| shundai | 1:844acc585d3d | 35 | |
| shundai | 1:844acc585d3d | 36 | |
| shundai | 5:1155a15f978c | 37 | // ゲインはモータードライバー固有のパラメータなのでflashに保存する， |
| shundai | 5:1155a15f978c | 38 | // 初期値としてデフォルトの値をdefineで定義する |
| shundai | 5:1155a15f978c | 39 | // theta feedback PID gain |
| shundai | 7:d71bef2faad7 | 40 | #define theta_Kp 1.0f |
| shundai | 7:d71bef2faad7 | 41 | #define theta_Ki 0.05f |
| shundai | 5:1155a15f978c | 42 | #define theta_Kd 0.00001f |
| shundai | 5:1155a15f978c | 43 | |
| shundai | 5:1155a15f978c | 44 | // omega feedback PID gain |
| shundai | 5:1155a15f978c | 45 | #define omega_Kp 0.05f |
| shundai | 7:d71bef2faad7 | 46 | #define omega_Ki 0.02f |
| shundai | 5:1155a15f978c | 47 | #define omega_Kd 0.0f |
| shundai | 5:1155a15f978c | 48 | |
| shundai | 4:40a764acc4ec | 49 | |
| shundai | 4:40a764acc4ec | 50 | #define ANGLE_THRESHOLD 2.0f //under 2.0 degree is decreaced |
| shundai | 1:844acc585d3d | 51 | |
| shundai | 5:1155a15f978c | 52 | // CAN PID define |
| shundai | 1:844acc585d3d | 53 | |
| shundai | 5:1155a15f978c | 54 | #define THETA_FEEDBACK_CCW 10 |
| shundai | 5:1155a15f978c | 55 | #define THETA_FEEDBACK_CW 11 |
| shundai | 5:1155a15f978c | 56 | #define OMEGA_FEEDBACK_CCW 20 |
| shundai | 5:1155a15f978c | 57 | #define OMEGA_FEEDBACK_CW 21 |
| shundai | 5:1155a15f978c | 58 | #define CURRENT_FEEDBACK_CCW 30 |
| shundai | 5:1155a15f978c | 59 | #define CURRENT_FEEDBACK_CW 31 |
| shundai | 1:844acc585d3d | 60 | |
| shundai | 5:1155a15f978c | 61 | #define MOTOR_STOP 0 |
| shundai | 5:1155a15f978c | 62 | #define THETA_FEEDBACK 10 |
| shundai | 5:1155a15f978c | 63 | #define OMEGA_FEEDBACK 20 |
| shundai | 5:1155a15f978c | 64 | #define CURRENT_FEEDBACK 30 |
| shundai | 1:844acc585d3d | 65 | |
| shundai | 5:1155a15f978c | 66 | // math value define |
| shundai | 1:844acc585d3d | 67 | #define M_PI 3.141593f |
| shundai | 1:844acc585d3d | 68 | |
| shundai | 5:1155a15f978c | 69 | // constant value |
| shundai | 1:844acc585d3d | 70 | #define ABLE 0 |
| shundai | 1:844acc585d3d | 71 | #define DISABLE 1 |
| shundai | 5:1155a15f978c | 72 | #define CCW 0 |
| shundai | 2:50e50ee8e08a | 73 | #define CW 1 |
| shundai | 1:844acc585d3d | 74 | |
| shundai | 5:1155a15f978c | 75 | // threshold value define |
| shundai | 1:844acc585d3d | 76 | #define MOTOR_TARGET_OMEGA_THRESHOLD 0.5f //[deg/s] |
| shundai | 5:1155a15f978c | 77 | #define MOTOR_TARGET_THETA_THRESHOLD 0.5f //[rpm] |
| shundai | 1:844acc585d3d | 78 | #define MOTOR_TARGET_CURRENT_THRESHOLD 0.1f //[A] |
| shundai | 1:844acc585d3d | 79 | |
| shundai | 5:1155a15f978c | 80 | // stm32f303k8t6 unique serial address |
| shundai | 1:844acc585d3d | 81 | #define uniqueSerialAdd_kl_freescale (unsigned long *)0x40048058 |
| shundai | 1:844acc585d3d | 82 | #define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8 |
| shundai | 1:844acc585d3d | 83 | #define uniqueSerialAddr uniqueSerialAddr_stm32 |
| shundai | 1:844acc585d3d | 84 | |
| shundai | 5:1155a15f978c | 85 | // controll value define |
| shundai | 5:1155a15f978c | 86 | #define PID_CONTROLL_PERIOD 8 //[ms] |
| shundai | 1:844acc585d3d | 87 | #define ENCODER_CONTROLL_PERIOD 8 //[ms] |
| shundai | 5:1155a15f978c | 88 | #define CAN_SEND_PERIOD 100 //[ms] |
| shundai | 5:1155a15f978c | 89 | #define MOTOR_FREUENCY 8000 //[Hz] |
| shundai | 5:1155a15f978c | 90 | #define CAN_FREQUENCY 100 //[kHz] |
| shundai | 5:1155a15f978c | 91 | #define SERIAL_BAUDLATE 115200 //[bps] |
| shundai | 5:1155a15f978c | 92 | #define CURRENT_SENSOR_OFFSET_COUNT 1000 // [count] |
| shundai | 1:844acc585d3d | 93 | |
| shundai | 5:1155a15f978c | 94 | // current sensor define |
| shundai | 1:844acc585d3d | 95 | #define CURRENT_SENSOR_VCC 3.3 //[V] |
| shundai | 1:844acc585d3d | 96 | #define CURRENT_SENSOR_RANGE 105 //[mv/A] |
| shundai | 4:40a764acc4ec | 97 | #define CURRENT_LIMIT 1.5 //[A] |
| shundai | 1:844acc585d3d | 98 | |
| shundai | 5:1155a15f978c | 99 | // turning by toukai robocon |
| shundai | 4:40a764acc4ec | 100 | #define MOTOR1_ID 393246 |
| shundai | 4:40a764acc4ec | 101 | #define MOTOR2_ID -2147418100 |
| shundai | 4:40a764acc4ec | 102 | #define MOTOR3_ID 196631 |
| shundai | 5:1155a15f978c | 103 | #define MOTOR4_ID 786469 |
| shundai | 5:1155a15f978c | 104 | #define MOTOR5_ID 1966107 |
| shundai | 5:1155a15f978c | 105 | #define MOTOR6_ID 1638418 |
| shundai | 1:844acc585d3d | 106 | |
| shundai | 1:844acc585d3d | 107 | |
| shundai | 1:844acc585d3d | 108 | //standerd define |
| shundai | 1:844acc585d3d | 109 | Serial pc(PB_6, PB_7, SERIAL_BAUDLATE); |
| shundai | 1:844acc585d3d | 110 | DigitalOut myled(LED1); //PB_3 |
| shundai | 1:844acc585d3d | 111 | DigitalIn userSW(PB_5); |
| shundai | 1:844acc585d3d | 112 | |
| shundai | 1:844acc585d3d | 113 | //motor define |
| shundai | 1:844acc585d3d | 114 | PwmOut PWM1(PA_4); |
| shundai | 1:844acc585d3d | 115 | PwmOut PWM2(PA_6); |
| shundai | 1:844acc585d3d | 116 | PwmOut PHASE(PB_0); |
| shundai | 1:844acc585d3d | 117 | DigitalOut SR(PA_7); |
| shundai | 5:1155a15f978c | 118 | PID thetaPID(theta_Kp, theta_Ki, theta_Kd, PID_CONTROLL_PERIOD*0.001f); |
| shundai | 5:1155a15f978c | 119 | PID omegaPID(omega_Kp, omega_Ki, omega_Kd, PID_CONTROLL_PERIOD*0.001f); |
| shundai | 5:1155a15f978c | 120 | |
| shundai | 5:1155a15f978c | 121 | //use encoder define |
| shundai | 5:1155a15f978c | 122 | |
| shundai | 5:1155a15f978c | 123 | /* |
| shundai | 5:1155a15f978c | 124 | pulse per revolution : 1000 |
| shundai | 5:1155a15f978c | 125 | phase : A, B, Z |
| shundai | 5:1155a15f978c | 126 | voltage : 10.8V - 20.0V |
| shundai | 5:1155a15f978c | 127 | */ |
| shundai | 7:d71bef2faad7 | 128 | #define ENCODER_TRD_S1000B |
| shundai | 7:d71bef2faad7 | 129 | //#define ENCODER_E6A2_CW5C |
| shundai | 5:1155a15f978c | 130 | |
| shundai | 5:1155a15f978c | 131 | #if defined ENCODER_TRD_S1000B |
| shundai | 5:1155a15f978c | 132 | #define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4) |
| shundai | 1:844acc585d3d | 133 | QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING); |
| shundai | 5:1155a15f978c | 134 | #endif |
| shundai | 5:1155a15f978c | 135 | |
| shundai | 5:1155a15f978c | 136 | #if defined ENCODER_E6A2_CW5C |
| shundai | 5:1155a15f978c | 137 | #define ENCODER_PULSE_PER_REVOLUTION 2000 //(500*4) |
| shundai | 5:1155a15f978c | 138 | QEI encoder(PA_3, PA_1, NC, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING); |
| shundai | 5:1155a15f978c | 139 | #endif |
| shundai | 5:1155a15f978c | 140 | |
| shundai | 5:1155a15f978c | 141 | //AnalogIn CPUtemp(ADC_TEMP); |
| shundai | 1:844acc585d3d | 142 | AnalogIn Current(PB_1); |
| shundai | 1:844acc585d3d | 143 | |
| shundai | 1:844acc585d3d | 144 | //CAN define |
| shundai | 1:844acc585d3d | 145 | CAN can(PA_11, PA_12); //RD(PA_11,D10),TD(PA_12,D2) |
| shundai | 1:844acc585d3d | 146 | CANMessage msg; |
| shundai | 1:844acc585d3d | 147 | |
| shundai | 1:844acc585d3d | 148 | struct CANdata |
| shundai | 1:844acc585d3d | 149 | { |
| shundai | 1:844acc585d3d | 150 | //int timeout; |
| shundai | 4:40a764acc4ec | 151 | int deviceID; |
| shundai | 1:844acc585d3d | 152 | char recvDATA[8]; |
| shundai | 1:844acc585d3d | 153 | char sendDATA[8]; |
| shundai | 1:844acc585d3d | 154 | }; |
| shundai | 1:844acc585d3d | 155 | |
| shundai | 1:844acc585d3d | 156 | struct CANdata MDCAN; |
| shundai | 1:844acc585d3d | 157 | |
| shundai | 1:844acc585d3d | 158 | |
| shundai | 5:1155a15f978c | 159 | |
| shundai | 1:844acc585d3d | 160 | //void CANtimeout(); |
| shundai | 1:844acc585d3d | 161 | //void CANtest(); |
| shundai | 1:844acc585d3d | 162 | |
| shundai | 4:40a764acc4ec | 163 | /* |
| shundai | 4:40a764acc4ec | 164 | int mode : select THETA_FEEDBACK(=1) or OMEGA_FEEDBACK(=2) |
| shundai | 4:40a764acc4ec | 165 | double target : enter the input value corresponding to the mode |
| shundai | 4:40a764acc4ec | 166 | */ |
| shundai | 2:50e50ee8e08a | 167 | void MotorDrive(int mode, double target); |
| shundai | 1:844acc585d3d | 168 | |
| shundai | 1:844acc585d3d | 169 | //void CurrentFunction(); |
| shundai | 1:844acc585d3d | 170 | InterruptIn userButton(PB_5); |
| shundai | 1:844acc585d3d | 171 | Ticker Encoder; |
| shundai | 5:1155a15f978c | 172 | Ticker SEND; |
| shundai | 7:d71bef2faad7 | 173 | Ticker DEMO; |
| shundai | 1:844acc585d3d | 174 | Timer ENCODER_timer; |
| shundai | 1:844acc585d3d | 175 | |
| shundai | 1:844acc585d3d | 176 | struct state |
| shundai | 1:844acc585d3d | 177 | { |
| shundai | 5:1155a15f978c | 178 | int PID; |
| shundai | 4:40a764acc4ec | 179 | double ThetaTarget; |
| shundai | 4:40a764acc4ec | 180 | double OmegaTarget; |
| shundai | 4:40a764acc4ec | 181 | double CurrentTarget; |
| shundai | 1:844acc585d3d | 182 | double theta; |
| shundai | 1:844acc585d3d | 183 | double omega; |
| shundai | 1:844acc585d3d | 184 | double current; |
| shundai | 1:844acc585d3d | 185 | }; |
| shundai | 1:844acc585d3d | 186 | |
| shundai | 1:844acc585d3d | 187 | struct state Motor; |
| shundai | 1:844acc585d3d | 188 | |
| shundai | 1:844acc585d3d | 189 | //Current Sensor setup |
| shundai | 1:844acc585d3d | 190 | double current_bias = 0.0; |
| shundai | 1:844acc585d3d | 191 | |
| shundai | 1:844acc585d3d | 192 | void Velocity(); |
| shundai | 1:844acc585d3d | 193 | void ButtonFunction(); |
| shundai | 7:d71bef2faad7 | 194 | void demo(); |
| shundai | 1:844acc585d3d | 195 | void SerialFunction(); |
| shundai | 1:844acc585d3d | 196 | void CANread(); |
| shundai | 5:1155a15f978c | 197 | void CANsend(); |
| shundai | 4:40a764acc4ec | 198 | void CurrentFunction(); |
| shundai | 5:1155a15f978c | 199 | char SerialGet(); |
| shundai | 1:844acc585d3d | 200 | |
| shundai | 4:40a764acc4ec | 201 | // 電流センサーをタイマー割込みにして，規定量を超えたら角速度の目標値を０にする． |
| shundai | 1:844acc585d3d | 202 | |
| shundai | 5:1155a15f978c | 203 | |
| shundai | 5:1155a15f978c | 204 | |
| shundai | 5:1155a15f978c | 205 | void loadFlash(uint32_t address, uint8_t *data, uint32_t size); |
| shundai | 5:1155a15f978c | 206 | void writeFlash(uint32_t address, uint8_t *data, uint32_t size); |
| shundai | 5:1155a15f978c | 207 | void eraseFlash(uint32_t address); |
| shundai | 5:1155a15f978c | 208 | |
| shundai | 7:d71bef2faad7 | 209 | //float buf = 0.0f; |
| shundai | 5:1155a15f978c | 210 | |
| shundai | 1:844acc585d3d | 211 | int main() |
| shundai | 1:844acc585d3d | 212 | { |
| shundai | 1:844acc585d3d | 213 | // MPU setup |
| shundai | 4:40a764acc4ec | 214 | DBG("MotorDriver_ver4.1 start! \r\n"); |
| shundai | 4:40a764acc4ec | 215 | DBG("/** MPU INFO **/ \r\n"); |
| shundai | 4:40a764acc4ec | 216 | DBG("Target PlatForm: STM32F303K8T6 \r\n"); |
| shundai | 4:40a764acc4ec | 217 | DBG("System Frequency: %d Hz \r\n", SystemCoreClock); |
| shundai | 4:40a764acc4ec | 218 | DBG("Program File: %s \r\n", __FILE__); |
| shundai | 4:40a764acc4ec | 219 | DBG("Compile Date: %s \r\n", __DATE__); |
| shundai | 4:40a764acc4ec | 220 | DBG("Compile Time: %s \r\n", __TIME__); |
| shundai | 1:844acc585d3d | 221 | |
| shundai | 1:844acc585d3d | 222 | //Unique ID starting addresses for most of STM32 microcontrollers. |
| shundai | 5:1155a15f978c | 223 | long *id = (long *)0x1FFFF7AC; |
| shundai | 1:844acc585d3d | 224 | |
| shundai | 4:40a764acc4ec | 225 | DBG("%d, %d, %d \r\n", id[0], id[1], id[2]); |
| shundai | 4:40a764acc4ec | 226 | |
| shundai | 4:40a764acc4ec | 227 | switch (id[0]) |
| shundai | 4:40a764acc4ec | 228 | { |
| shundai | 4:40a764acc4ec | 229 | case MOTOR1_ID: |
| shundai | 4:40a764acc4ec | 230 | MDCAN.deviceID = 11; |
| shundai | 4:40a764acc4ec | 231 | DBG("\r\n ==MOTOR1== \r\n\r\n"); |
| shundai | 4:40a764acc4ec | 232 | break; |
| shundai | 4:40a764acc4ec | 233 | |
| shundai | 4:40a764acc4ec | 234 | case MOTOR2_ID: |
| shundai | 4:40a764acc4ec | 235 | MDCAN.deviceID = 12; |
| shundai | 4:40a764acc4ec | 236 | DBG("\r\n ==MOTOR2== \r\n\r\n"); |
| shundai | 4:40a764acc4ec | 237 | break; |
| shundai | 4:40a764acc4ec | 238 | |
| shundai | 4:40a764acc4ec | 239 | case MOTOR3_ID: |
| shundai | 4:40a764acc4ec | 240 | MDCAN.deviceID = 13; |
| shundai | 4:40a764acc4ec | 241 | DBG("\r\n ==MOTOR3== \r\n\r\n"); |
| shundai | 4:40a764acc4ec | 242 | break; |
| shundai | 1:844acc585d3d | 243 | |
| shundai | 4:40a764acc4ec | 244 | case MOTOR4_ID: |
| shundai | 4:40a764acc4ec | 245 | MDCAN.deviceID = 14; |
| shundai | 4:40a764acc4ec | 246 | DBG("\r\n ==MOTOR4== \r\n\r\n"); |
| shundai | 4:40a764acc4ec | 247 | break; |
| shundai | 4:40a764acc4ec | 248 | |
| shundai | 4:40a764acc4ec | 249 | case MOTOR5_ID: |
| shundai | 4:40a764acc4ec | 250 | MDCAN.deviceID = 15; |
| shundai | 4:40a764acc4ec | 251 | DBG("\r\n ==MOTOR5== \r\n\r\n"); |
| shundai | 4:40a764acc4ec | 252 | break; |
| shundai | 4:40a764acc4ec | 253 | |
| shundai | 4:40a764acc4ec | 254 | case MOTOR6_ID: |
| shundai | 4:40a764acc4ec | 255 | MDCAN.deviceID = 16; |
| shundai | 4:40a764acc4ec | 256 | DBG("\r\n ==MOTOR6== \r\n\r\n"); |
| shundai | 4:40a764acc4ec | 257 | break; |
| shundai | 4:40a764acc4ec | 258 | |
| shundai | 4:40a764acc4ec | 259 | default: |
| shundai | 4:40a764acc4ec | 260 | |
| shundai | 4:40a764acc4ec | 261 | while(1) |
| shundai | 4:40a764acc4ec | 262 | { |
| shundai | 5:1155a15f978c | 263 | DBG("deviceID none define. It does not work!\r\n"); |
| shundai | 4:40a764acc4ec | 264 | } |
| shundai | 4:40a764acc4ec | 265 | } |
| shundai | 1:844acc585d3d | 266 | |
| shundai | 4:40a764acc4ec | 267 | DBG("deviceID : %d \r\n", MDCAN.deviceID); |
| shundai | 5:1155a15f978c | 268 | |
| shundai | 5:1155a15f978c | 269 | // user LED Indicator |
| shundai | 1:844acc585d3d | 270 | myled = 1; |
| shundai | 1:844acc585d3d | 271 | wait(1.0); |
| shundai | 1:844acc585d3d | 272 | myled = 0; |
| shundai | 1:844acc585d3d | 273 | |
| shundai | 1:844acc585d3d | 274 | // CAN setup |
| shundai | 5:1155a15f978c | 275 | can.frequency(CAN_FREQUENCY*1000); //ROS default bps:100kbps -> 500kbps |
| shundai | 4:40a764acc4ec | 276 | can.mode(CAN::Normal); |
| shundai | 4:40a764acc4ec | 277 | //can.filter(MDCAN.deviceID, 0x0000000F, CANStandard, 14); |
| shundai | 4:40a764acc4ec | 278 | //can.reset(); //delete receive buffer |
| shundai | 5:1155a15f978c | 279 | |
| shundai | 1:844acc585d3d | 280 | // motor signal setup |
| shundai | 1:844acc585d3d | 281 | PWM1.period(1.0f/MOTOR_FREUENCY); |
| shundai | 1:844acc585d3d | 282 | PWM2.period(1.0f/MOTOR_FREUENCY); |
| shundai | 4:40a764acc4ec | 283 | PHASE.period(1.0f/MOTOR_FREUENCY); |
| shundai | 1:844acc585d3d | 284 | PWM1 = 0.0f; |
| shundai | 1:844acc585d3d | 285 | PWM2 = 0.0f; |
| shundai | 1:844acc585d3d | 286 | PHASE = 0.0f; |
| shundai | 1:844acc585d3d | 287 | SR = 0; |
| shundai | 4:40a764acc4ec | 288 | |
| shundai | 4:40a764acc4ec | 289 | // PID position setup |
| shundai | 5:1155a15f978c | 290 | thetaPID.setInputLimits(-360.0f*100.0f, 360.0f*100.0f); //[deg] |
| shundai | 5:1155a15f978c | 291 | thetaPID.setOutputLimits(-240.0f, 240.0f); //[rpm] |
| shundai | 4:40a764acc4ec | 292 | thetaPID.setBias(0.0); |
| shundai | 4:40a764acc4ec | 293 | thetaPID.setMode(AUTO_MODE); |
| shundai | 1:844acc585d3d | 294 | |
| shundai | 5:1155a15f978c | 295 | // PID omega setup |
| shundai | 5:1155a15f978c | 296 | omegaPID.setInputLimits(0.0, 240.0f*0.9f); //[rpm] |
| shundai | 5:1155a15f978c | 297 | omegaPID.setOutputLimits(0.0f, 1.0f); //[duty] |
| shundai | 5:1155a15f978c | 298 | omegaPID.setBias(0.0); |
| shundai | 5:1155a15f978c | 299 | omegaPID.setMode(AUTO_MODE); |
| shundai | 5:1155a15f978c | 300 | |
| shundai | 1:844acc585d3d | 301 | // current sensor setup |
| shundai | 1:844acc585d3d | 302 | for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++) |
| shundai | 1:844acc585d3d | 303 | { |
| shundai | 1:844acc585d3d | 304 | current_bias += Current.read(); |
| shundai | 1:844acc585d3d | 305 | } |
| shundai | 1:844acc585d3d | 306 | |
| shundai | 1:844acc585d3d | 307 | current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT; |
| shundai | 4:40a764acc4ec | 308 | DBG("current_bias_average: %f \r\n", current_bias); |
| shundai | 5:1155a15f978c | 309 | |
| shundai | 1:844acc585d3d | 310 | // interrupt start function |
| shundai | 1:844acc585d3d | 311 | userButton.mode(PullUp); |
| shundai | 1:844acc585d3d | 312 | userButton.fall(ButtonFunction); |
| shundai | 1:844acc585d3d | 313 | pc.attach(SerialFunction, Serial::RxIrq); |
| shundai | 1:844acc585d3d | 314 | can.attach(CANread, CAN::RxIrq); |
| shundai | 5:1155a15f978c | 315 | SEND.attach(CANsend, CAN_SEND_PERIOD*0.001f); |
| shundai | 1:844acc585d3d | 316 | ENCODER_timer.start(); |
| shundai | 1:844acc585d3d | 317 | Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms |
| shundai | 1:844acc585d3d | 318 | //NVIC_SetPriority(TIMER3_IRQn, 1); |
| shundai | 1:844acc585d3d | 319 | |
| shundai | 4:40a764acc4ec | 320 | //initial target |
| shundai | 5:1155a15f978c | 321 | Motor.PID = OMEGA_FEEDBACK; // initial drive mode is stop. |
| shundai | 5:1155a15f978c | 322 | Motor.ThetaTarget = 0.0f; // initial theta[deg] |
| shundai | 5:1155a15f978c | 323 | Motor.OmegaTarget = 0.0f; // initial omega[rpm] |
| shundai | 4:40a764acc4ec | 324 | |
| shundai | 1:844acc585d3d | 325 | while(1) |
| shundai | 1:844acc585d3d | 326 | { |
| shundai | 7:d71bef2faad7 | 327 | /* |
| shundai | 5:1155a15f978c | 328 | pc.printf("%f \r\n", buf); |
| shundai | 5:1155a15f978c | 329 | |
| shundai | 5:1155a15f978c | 330 | if(buf > 0.0f) |
| shundai | 5:1155a15f978c | 331 | { |
| shundai | 5:1155a15f978c | 332 | PWM1 = 0.0f; |
| shundai | 5:1155a15f978c | 333 | PWM2 = buf; |
| shundai | 5:1155a15f978c | 334 | } |
| shundai | 5:1155a15f978c | 335 | |
| shundai | 5:1155a15f978c | 336 | else if(buf <= 0.0f) |
| shundai | 5:1155a15f978c | 337 | { |
| shundai | 5:1155a15f978c | 338 | PWM1 = buf; |
| shundai | 5:1155a15f978c | 339 | PWM2 = 0.0f; |
| shundai | 5:1155a15f978c | 340 | } |
| shundai | 7:d71bef2faad7 | 341 | */ |
| shundai | 2:50e50ee8e08a | 342 | //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f); |
| shundai | 1:844acc585d3d | 343 | |
| shundai | 4:40a764acc4ec | 344 | //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); |
| shundai | 1:844acc585d3d | 345 | |
| shundai | 5:1155a15f978c | 346 | switch(Motor.PID) |
| shundai | 5:1155a15f978c | 347 | { |
| shundai | 5:1155a15f978c | 348 | case THETA_FEEDBACK: |
| shundai | 5:1155a15f978c | 349 | |
| shundai | 5:1155a15f978c | 350 | //DBG("THETA_FEEDBACK \r\n"); |
| shundai | 5:1155a15f978c | 351 | MotorDrive(THETA_FEEDBACK, Motor.ThetaTarget); |
| shundai | 5:1155a15f978c | 352 | break; |
| shundai | 5:1155a15f978c | 353 | |
| shundai | 5:1155a15f978c | 354 | case OMEGA_FEEDBACK: |
| shundai | 5:1155a15f978c | 355 | |
| shundai | 5:1155a15f978c | 356 | //DBG("OMEGA_FEEDBACK \r\n"); |
| shundai | 5:1155a15f978c | 357 | MotorDrive(OMEGA_FEEDBACK, Motor.OmegaTarget); |
| shundai | 5:1155a15f978c | 358 | break; |
| shundai | 5:1155a15f978c | 359 | |
| shundai | 5:1155a15f978c | 360 | case CURRENT_FEEDBACK: |
| shundai | 5:1155a15f978c | 361 | |
| shundai | 5:1155a15f978c | 362 | //DBG("CURRENT_FEEDBACK \r\n"); |
| shundai | 5:1155a15f978c | 363 | MotorDrive(CURRENT_FEEDBACK, Motor.CurrentTarget); |
| shundai | 5:1155a15f978c | 364 | |
| shundai | 5:1155a15f978c | 365 | case MOTOR_STOP: |
| shundai | 5:1155a15f978c | 366 | |
| shundai | 5:1155a15f978c | 367 | //DBG("MOTOR_STOP \r\n"); |
| shundai | 5:1155a15f978c | 368 | MotorDrive(OMEGA_FEEDBACK, 0.0f); |
| shundai | 5:1155a15f978c | 369 | break; |
| shundai | 5:1155a15f978c | 370 | |
| shundai | 5:1155a15f978c | 371 | default: // the others PID is stop. |
| shundai | 5:1155a15f978c | 372 | |
| shundai | 5:1155a15f978c | 373 | MotorDrive(OMEGA_FEEDBACK, 0.0f); |
| shundai | 5:1155a15f978c | 374 | break; |
| shundai | 5:1155a15f978c | 375 | } |
| shundai | 5:1155a15f978c | 376 | |
| shundai | 5:1155a15f978c | 377 | //pc.printf("PID[%d], TT[%6.3f deg], OT[%6.3f rpm], T[%6.3f deg], O[%6.3f rpm] \r\n", Motor.PID, Motor.ThetaTarget, Motor.OmegaTarget, Motor.theta, Motor.omega); |
| shundai | 5:1155a15f978c | 378 | pc.printf("T[%6.3f deg], O[%6.3f rpm] \r\n", Motor.theta, Motor.omega); |
| shundai | 2:50e50ee8e08a | 379 | |
| shundai | 4:40a764acc4ec | 380 | //DBG("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,thetaPID.compute()); |
| shundai | 1:844acc585d3d | 381 | //CurrentFunction(); |
| shundai | 5:1155a15f978c | 382 | |
| shundai | 1:844acc585d3d | 383 | } |
| shundai | 1:844acc585d3d | 384 | } |
| shundai | 1:844acc585d3d | 385 | |
| shundai | 1:844acc585d3d | 386 | void SerialFunction() |
| shundai | 1:844acc585d3d | 387 | { |
| shundai | 1:844acc585d3d | 388 | if(pc.readable() == 1) |
| shundai | 1:844acc585d3d | 389 | { |
| shundai | 1:844acc585d3d | 390 | char data = pc.getc(); |
| shundai | 4:40a764acc4ec | 391 | DBG("get data:%c \r\n", data); |
| shundai | 5:1155a15f978c | 392 | |
| shundai | 1:844acc585d3d | 393 | switch (data) |
| shundai | 1:844acc585d3d | 394 | { |
| shundai | 2:50e50ee8e08a | 395 | case 'p': |
| shundai | 7:d71bef2faad7 | 396 | Motor.PID = THETA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 397 | Motor.ThetaTarget += 30.0f; |
| shundai | 7:d71bef2faad7 | 398 | //buf += 0.05f; |
| shundai | 4:40a764acc4ec | 399 | DBG("get 'p' \r\n"); |
| shundai | 2:50e50ee8e08a | 400 | |
| shundai | 2:50e50ee8e08a | 401 | break; |
| shundai | 2:50e50ee8e08a | 402 | |
| shundai | 2:50e50ee8e08a | 403 | case 'l': |
| shundai | 7:d71bef2faad7 | 404 | Motor.PID = THETA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 405 | Motor.ThetaTarget -= 30.0f; |
| shundai | 7:d71bef2faad7 | 406 | //buf -= 0.05f; |
| shundai | 4:40a764acc4ec | 407 | DBG("get 'l' \r\n"); |
| shundai | 2:50e50ee8e08a | 408 | |
| shundai | 2:50e50ee8e08a | 409 | break; |
| shundai | 5:1155a15f978c | 410 | |
| shundai | 5:1155a15f978c | 411 | case 'o': |
| shundai | 5:1155a15f978c | 412 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 413 | Motor.OmegaTarget += 30.0f; |
| shundai | 5:1155a15f978c | 414 | DBG("get 'q' \r\n"); |
| shundai | 5:1155a15f978c | 415 | |
| shundai | 5:1155a15f978c | 416 | break; |
| shundai | 2:50e50ee8e08a | 417 | |
| shundai | 5:1155a15f978c | 418 | case 'k': |
| shundai | 5:1155a15f978c | 419 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 420 | Motor.OmegaTarget -= 30.0f; |
| shundai | 5:1155a15f978c | 421 | DBG("get 'a' \r\n"); |
| shundai | 1:844acc585d3d | 422 | |
| shundai | 5:1155a15f978c | 423 | break; |
| shundai | 5:1155a15f978c | 424 | |
| shundai | 1:844acc585d3d | 425 | case 'r': |
| shundai | 1:844acc585d3d | 426 | |
| shundai | 4:40a764acc4ec | 427 | DBG("\r\nSystem Reset! \r\n"); |
| shundai | 1:844acc585d3d | 428 | NVIC_SystemReset(); |
| shundai | 1:844acc585d3d | 429 | |
| shundai | 1:844acc585d3d | 430 | break; |
| shundai | 5:1155a15f978c | 431 | |
| shundai | 5:1155a15f978c | 432 | case 'f': //PID free (PWM only) |
| shundai | 5:1155a15f978c | 433 | |
| shundai | 5:1155a15f978c | 434 | |
| shundai | 5:1155a15f978c | 435 | /* |
| shundai | 5:1155a15f978c | 436 | while(1) |
| shundai | 5:1155a15f978c | 437 | { |
| shundai | 5:1155a15f978c | 438 | SR = ABLE; |
| shundai | 5:1155a15f978c | 439 | PHASE = (float)CW; |
| shundai | 5:1155a15f978c | 440 | PWM1 = 0.05f; |
| shundai | 5:1155a15f978c | 441 | PWM2 = 1.0f; |
| shundai | 5:1155a15f978c | 442 | } |
| shundai | 5:1155a15f978c | 443 | */ |
| shundai | 5:1155a15f978c | 444 | |
| shundai | 1:844acc585d3d | 445 | default: |
| shundai | 1:844acc585d3d | 446 | |
| shundai | 4:40a764acc4ec | 447 | DBG("The others key recieve \r\n"); |
| shundai | 1:844acc585d3d | 448 | |
| shundai | 1:844acc585d3d | 449 | break; |
| shundai | 1:844acc585d3d | 450 | } |
| shundai | 1:844acc585d3d | 451 | } |
| shundai | 1:844acc585d3d | 452 | } |
| shundai | 7:d71bef2faad7 | 453 | /* |
| shundai | 1:844acc585d3d | 454 | void ButtonFunction() |
| shundai | 1:844acc585d3d | 455 | { |
| shundai | 5:1155a15f978c | 456 | //serial menu printf |
| shundai | 4:40a764acc4ec | 457 | DBG("setup open \r\n"); |
| shundai | 5:1155a15f978c | 458 | DBG("1: Theta Controll\r\n"); |
| shundai | 5:1155a15f978c | 459 | DBG("2: Omega Controll\r\n"); |
| shundai | 5:1155a15f978c | 460 | DBG("3: Exit setup menu...\r\n"); |
| shundai | 5:1155a15f978c | 461 | |
| shundai | 5:1155a15f978c | 462 | DBG("Send to MD11 test CAN data\r\n"); |
| shundai | 5:1155a15f978c | 463 | |
| shundai | 5:1155a15f978c | 464 | MDCAN.sendDATA[0] = 21; // omega FB CCw |
| shundai | 5:1155a15f978c | 465 | MDCAN.sendDATA[1] = 60; // 60 rpm |
| shundai | 5:1155a15f978c | 466 | MDCAN.sendDATA[2] = 0; |
| shundai | 5:1155a15f978c | 467 | MDCAN.sendDATA[3] = 0; |
| shundai | 5:1155a15f978c | 468 | MDCAN.sendDATA[4] = 0; |
| shundai | 5:1155a15f978c | 469 | MDCAN.sendDATA[5] = 0; |
| shundai | 5:1155a15f978c | 470 | MDCAN.sendDATA[6] = 0; |
| shundai | 5:1155a15f978c | 471 | MDCAN.sendDATA[7] = 0; |
| shundai | 5:1155a15f978c | 472 | |
| shundai | 5:1155a15f978c | 473 | if(!can.write(CANMessage(12, MDCAN.sendDATA, 8, CANData, CANStandard))) |
| shundai | 5:1155a15f978c | 474 | { |
| shundai | 5:1155a15f978c | 475 | DBG("CAN send error! \r\n"); |
| shundai | 5:1155a15f978c | 476 | } |
| shundai | 1:844acc585d3d | 477 | |
| shundai | 1:844acc585d3d | 478 | //CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能 |
| shundai | 1:844acc585d3d | 479 | |
| shundai | 1:844acc585d3d | 480 | //ユーザーボタンを押したらモータを台形加速させるプログラムを作る |
| shundai | 1:844acc585d3d | 481 | |
| shundai | 5:1155a15f978c | 482 | //ボタン情報を割込みで入力させる関数を用意してボタン情報を入力する． |
| shundai | 5:1155a15f978c | 483 | |
| shundai | 5:1155a15f978c | 484 | //モータが駆動し始める電流を測定する．（パルスが増えるまでＰＷＭを微小増加させる） |
| shundai | 7:d71bef2faad7 | 485 | |
| shundai | 5:1155a15f978c | 486 | Motor.OmegaTarget = 60.0f; //[rpm] |
| shundai | 5:1155a15f978c | 487 | |
| shundai | 1:844acc585d3d | 488 | while(1) |
| shundai | 1:844acc585d3d | 489 | { |
| shundai | 5:1155a15f978c | 490 | // motor stop |
| shundai | 5:1155a15f978c | 491 | PWM1 = 0.0f; |
| shundai | 5:1155a15f978c | 492 | PWM2 = 0.0f; |
| shundai | 1:844acc585d3d | 493 | } |
| shundai | 7:d71bef2faad7 | 494 | |
| shundai | 7:d71bef2faad7 | 495 | } |
| shundai | 7:d71bef2faad7 | 496 | */ |
| shundai | 7:d71bef2faad7 | 497 | |
| shundai | 7:d71bef2faad7 | 498 | |
| shundai | 7:d71bef2faad7 | 499 | void ButtonFunction() |
| shundai | 7:d71bef2faad7 | 500 | { |
| shundai | 7:d71bef2faad7 | 501 | Motor.OmegaTarget = 0.0f; //motor stop |
| shundai | 7:d71bef2faad7 | 502 | |
| shundai | 7:d71bef2faad7 | 503 | DEMO.attach(demo, 0.2); |
| shundai | 7:d71bef2faad7 | 504 | |
| shundai | 7:d71bef2faad7 | 505 | DBG("setup open \r\n"); |
| shundai | 7:d71bef2faad7 | 506 | DBG("1:change the CAN device ID \r\n"); |
| shundai | 7:d71bef2faad7 | 507 | DBG("2:change the CAN device ID \r\n"); |
| shundai | 7:d71bef2faad7 | 508 | |
| shundai | 7:d71bef2faad7 | 509 | //CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能 |
| shundai | 7:d71bef2faad7 | 510 | |
| shundai | 7:d71bef2faad7 | 511 | //ユーザーボタンを押したらモータを台形加速させるプログラムを作る |
| shundai | 7:d71bef2faad7 | 512 | |
| shundai | 7:d71bef2faad7 | 513 | } |
| shundai | 7:d71bef2faad7 | 514 | |
| shundai | 7:d71bef2faad7 | 515 | void demo() |
| shundai | 7:d71bef2faad7 | 516 | { |
| shundai | 7:d71bef2faad7 | 517 | static int phase; |
| shundai | 7:d71bef2faad7 | 518 | |
| shundai | 7:d71bef2faad7 | 519 | switch (phase%8) |
| shundai | 7:d71bef2faad7 | 520 | { |
| shundai | 7:d71bef2faad7 | 521 | case 0: |
| shundai | 7:d71bef2faad7 | 522 | |
| shundai | 7:d71bef2faad7 | 523 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 524 | Motor.OmegaTarget = 30.0f; |
| shundai | 7:d71bef2faad7 | 525 | |
| shundai | 7:d71bef2faad7 | 526 | break; |
| shundai | 7:d71bef2faad7 | 527 | |
| shundai | 7:d71bef2faad7 | 528 | case 1: |
| shundai | 7:d71bef2faad7 | 529 | |
| shundai | 7:d71bef2faad7 | 530 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 531 | Motor.OmegaTarget = 60.0f; |
| shundai | 7:d71bef2faad7 | 532 | |
| shundai | 7:d71bef2faad7 | 533 | break; |
| shundai | 7:d71bef2faad7 | 534 | |
| shundai | 7:d71bef2faad7 | 535 | case 2: |
| shundai | 7:d71bef2faad7 | 536 | |
| shundai | 7:d71bef2faad7 | 537 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 538 | Motor.OmegaTarget = 30.0f; |
| shundai | 7:d71bef2faad7 | 539 | |
| shundai | 7:d71bef2faad7 | 540 | break; |
| shundai | 7:d71bef2faad7 | 541 | |
| shundai | 7:d71bef2faad7 | 542 | case 3: |
| shundai | 7:d71bef2faad7 | 543 | |
| shundai | 7:d71bef2faad7 | 544 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 545 | Motor.OmegaTarget = 0.0f; |
| shundai | 7:d71bef2faad7 | 546 | |
| shundai | 7:d71bef2faad7 | 547 | break; |
| shundai | 7:d71bef2faad7 | 548 | |
| shundai | 7:d71bef2faad7 | 549 | case 4: |
| shundai | 7:d71bef2faad7 | 550 | |
| shundai | 7:d71bef2faad7 | 551 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 552 | Motor.OmegaTarget = -30.0f; |
| shundai | 7:d71bef2faad7 | 553 | |
| shundai | 7:d71bef2faad7 | 554 | break; |
| shundai | 7:d71bef2faad7 | 555 | |
| shundai | 7:d71bef2faad7 | 556 | case 5: |
| shundai | 7:d71bef2faad7 | 557 | |
| shundai | 7:d71bef2faad7 | 558 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 559 | Motor.OmegaTarget = -60.0f; |
| shundai | 7:d71bef2faad7 | 560 | |
| shundai | 7:d71bef2faad7 | 561 | break; |
| shundai | 7:d71bef2faad7 | 562 | |
| shundai | 7:d71bef2faad7 | 563 | case 6: |
| shundai | 7:d71bef2faad7 | 564 | |
| shundai | 7:d71bef2faad7 | 565 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 566 | Motor.OmegaTarget = -30.0f; |
| shundai | 7:d71bef2faad7 | 567 | |
| shundai | 7:d71bef2faad7 | 568 | break; |
| shundai | 7:d71bef2faad7 | 569 | |
| shundai | 7:d71bef2faad7 | 570 | case 7: |
| shundai | 7:d71bef2faad7 | 571 | |
| shundai | 7:d71bef2faad7 | 572 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 7:d71bef2faad7 | 573 | Motor.OmegaTarget = 0.0f; |
| shundai | 7:d71bef2faad7 | 574 | |
| shundai | 7:d71bef2faad7 | 575 | break; |
| shundai | 7:d71bef2faad7 | 576 | } |
| shundai | 7:d71bef2faad7 | 577 | |
| shundai | 7:d71bef2faad7 | 578 | phase++; |
| shundai | 1:844acc585d3d | 579 | } |
| shundai | 1:844acc585d3d | 580 | |
| shundai | 1:844acc585d3d | 581 | void CANread() |
| shundai | 1:844acc585d3d | 582 | { |
| shundai | 5:1155a15f978c | 583 | // 1. when data received. |
| shundai | 5:1155a15f978c | 584 | // 2. when CAN data ID equal my ID |
| shundai | 5:1155a15f978c | 585 | // 3. when CAN data format is Standerd |
| shundai | 5:1155a15f978c | 586 | // 4. Not the same as the one received in the past |
| shundai | 5:1155a15f978c | 587 | // 同じものは受信しないプログラムを消したら通信できた446re |
| shundai | 5:1155a15f978c | 588 | if(can.read(msg) != 0 && |
| shundai | 5:1155a15f978c | 589 | MDCAN.deviceID == msg.id && |
| shundai | 5:1155a15f978c | 590 | msg.format == 0 ) |
| shundai | 1:844acc585d3d | 591 | { |
| shundai | 4:40a764acc4ec | 592 | DBG("CAN recieve \r\n"); |
| shundai | 1:844acc585d3d | 593 | myled = !myled; |
| shundai | 4:40a764acc4ec | 594 | |
| shundai | 4:40a764acc4ec | 595 | DBG("recv ID : %d", msg.id); |
| shundai | 1:844acc585d3d | 596 | |
| shundai | 1:844acc585d3d | 597 | for(int i = 0 ; i < 8 ; i++) |
| shundai | 1:844acc585d3d | 598 | { |
| shundai | 1:844acc585d3d | 599 | MDCAN.recvDATA[i] = msg.data[i]; |
| shundai | 4:40a764acc4ec | 600 | DBG("[%d] ", MDCAN.recvDATA[i]); |
| shundai | 1:844acc585d3d | 601 | } |
| shundai | 1:844acc585d3d | 602 | |
| shundai | 4:40a764acc4ec | 603 | DBG("\r\n"); |
| shundai | 1:844acc585d3d | 604 | |
| shundai | 4:40a764acc4ec | 605 | switch(MDCAN.recvDATA[0]) //check PID |
| shundai | 1:844acc585d3d | 606 | { |
| shundai | 5:1155a15f978c | 607 | //送られてくるデータはかならず正なので１バイト目のCCW，CWじょうほうを目標値に付け加える． |
| shundai | 5:1155a15f978c | 608 | case THETA_FEEDBACK_CW: //MD data send |
| shundai | 5:1155a15f978c | 609 | |
| shundai | 5:1155a15f978c | 610 | Motor.PID = THETA_FEEDBACK; |
| shundai | 5:1155a15f978c | 611 | Motor.ThetaTarget = (double)MDCAN.recvDATA[1]; |
| shundai | 5:1155a15f978c | 612 | DBG("theta CW FB CAN \r\n"); |
| shundai | 5:1155a15f978c | 613 | break; |
| shundai | 5:1155a15f978c | 614 | |
| shundai | 5:1155a15f978c | 615 | case THETA_FEEDBACK_CCW: //MD data send |
| shundai | 4:40a764acc4ec | 616 | |
| shundai | 5:1155a15f978c | 617 | Motor.PID = THETA_FEEDBACK; |
| shundai | 5:1155a15f978c | 618 | Motor.ThetaTarget = -1.0f*(double)MDCAN.recvDATA[1]; |
| shundai | 5:1155a15f978c | 619 | DBG("theta CCW FB CAN \r\n"); |
| shundai | 1:844acc585d3d | 620 | break; |
| shundai | 1:844acc585d3d | 621 | |
| shundai | 5:1155a15f978c | 622 | case OMEGA_FEEDBACK_CW: |
| shundai | 1:844acc585d3d | 623 | |
| shundai | 5:1155a15f978c | 624 | // 1 rpm(0.0166 rps) |
| shundai | 5:1155a15f978c | 625 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 4:40a764acc4ec | 626 | Motor.OmegaTarget = (double)MDCAN.recvDATA[1]; |
| shundai | 5:1155a15f978c | 627 | DBG("omega CW FB CAN \r\n"); |
| shundai | 5:1155a15f978c | 628 | break; |
| shundai | 5:1155a15f978c | 629 | |
| shundai | 5:1155a15f978c | 630 | case OMEGA_FEEDBACK_CCW: |
| shundai | 5:1155a15f978c | 631 | |
| shundai | 5:1155a15f978c | 632 | // 1 rpm(0.0166 rps) |
| shundai | 5:1155a15f978c | 633 | Motor.PID = OMEGA_FEEDBACK; |
| shundai | 5:1155a15f978c | 634 | Motor.OmegaTarget = -1.0f*(double)MDCAN.recvDATA[1]; |
| shundai | 5:1155a15f978c | 635 | DBG("omega CCW FB CAN \r\n"); |
| shundai | 1:844acc585d3d | 636 | break; |
| shundai | 1:844acc585d3d | 637 | |
| shundai | 1:844acc585d3d | 638 | default: |
| shundai | 4:40a764acc4ec | 639 | |
| shundai | 4:40a764acc4ec | 640 | DBG("recv CAN PID error \r\n"); |
| shundai | 1:844acc585d3d | 641 | |
| shundai | 1:844acc585d3d | 642 | break; |
| shundai | 1:844acc585d3d | 643 | } |
| shundai | 1:844acc585d3d | 644 | } |
| shundai | 1:844acc585d3d | 645 | } |
| shundai | 1:844acc585d3d | 646 | |
| shundai | 1:844acc585d3d | 647 | void Velocity() |
| shundai | 1:844acc585d3d | 648 | { |
| shundai | 1:844acc585d3d | 649 | static int pulse; //old pulse |
| shundai | 1:844acc585d3d | 650 | |
| shundai | 5:1155a15f978c | 651 | Motor.omega = 60.0f*((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read(); |
| shundai | 1:844acc585d3d | 652 | pulse = encoder.getPulses(); //pulse update |
| shundai | 1:844acc585d3d | 653 | |
| shundai | 1:844acc585d3d | 654 | Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f; |
| shundai | 1:844acc585d3d | 655 | |
| shundai | 1:844acc585d3d | 656 | ENCODER_timer.reset(); //timer reset:t=0s |
| shundai | 5:1155a15f978c | 657 | if(Motor.omega > 0) |
| shundai | 5:1155a15f978c | 658 | { |
| shundai | 5:1155a15f978c | 659 | MDCAN.sendDATA[0] = OMEGA_FEEDBACK_CW; |
| shundai | 5:1155a15f978c | 660 | } |
| shundai | 1:844acc585d3d | 661 | |
| shundai | 5:1155a15f978c | 662 | else |
| shundai | 5:1155a15f978c | 663 | { |
| shundai | 5:1155a15f978c | 664 | MDCAN.sendDATA[0] = OMEGA_FEEDBACK_CCW; |
| shundai | 5:1155a15f978c | 665 | } |
| shundai | 5:1155a15f978c | 666 | //pc.printf("%d \r\n", abs(int(Motor.omega))); |
| shundai | 5:1155a15f978c | 667 | //MDCAN.sendDATA[1] = abs(int(Motor.omega)); |
| shundai | 5:1155a15f978c | 668 | //MDCAN.sendDATA[2] = 0; |
| shundai | 5:1155a15f978c | 669 | //MDCAN.sendDATA[3] = 0; |
| shundai | 5:1155a15f978c | 670 | //MDCAN.sendDATA[4] = 0; |
| shundai | 5:1155a15f978c | 671 | //MDCAN.sendDATA[5] = 0; |
| shundai | 5:1155a15f978c | 672 | //MDCAN.sendDATA[6] = 0; |
| shundai | 5:1155a15f978c | 673 | //MDCAN.sendDATA[7] = 0; |
| shundai | 5:1155a15f978c | 674 | |
| shundai | 5:1155a15f978c | 675 | /* |
| shundai | 5:1155a15f978c | 676 | if(!can.write(CANMessage(MDCAN.deviceID*10, MDCAN.sendDATA, 8, CANData, CANExtended))) |
| shundai | 5:1155a15f978c | 677 | { |
| shundai | 5:1155a15f978c | 678 | DBG("CAN send error! \r\n"); |
| shundai | 5:1155a15f978c | 679 | } |
| shundai | 4:40a764acc4ec | 680 | */ |
| shundai | 5:1155a15f978c | 681 | |
| shundai | 4:40a764acc4ec | 682 | //DBG("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse); |
| shundai | 1:844acc585d3d | 683 | } |
| shundai | 1:844acc585d3d | 684 | |
| shundai | 5:1155a15f978c | 685 | void CANsend() |
| shundai | 1:844acc585d3d | 686 | { |
| shundai | 5:1155a15f978c | 687 | //CANSenｄがデータ集めて送信（50mstoka） |
| shundai | 4:40a764acc4ec | 688 | //__disable_irq(); |
| shundai | 4:40a764acc4ec | 689 | //Encoder.detach(); |
| shundai | 1:844acc585d3d | 690 | |
| shundai | 4:40a764acc4ec | 691 | //data -> Can data |
| shundai | 4:40a764acc4ec | 692 | |
| shundai | 5:1155a15f978c | 693 | switch (MDCAN.sendDATA[0]) |
| shundai | 5:1155a15f978c | 694 | { |
| shundai | 5:1155a15f978c | 695 | case THETA_FEEDBACK: |
| shundai | 5:1155a15f978c | 696 | MDCAN.sendDATA[1] = abs(int(Motor.theta)); |
| shundai | 5:1155a15f978c | 697 | break; |
| shundai | 5:1155a15f978c | 698 | |
| shundai | 5:1155a15f978c | 699 | case OMEGA_FEEDBACK: |
| shundai | 5:1155a15f978c | 700 | MDCAN.sendDATA[1] = abs(int(Motor.omega)); |
| shundai | 5:1155a15f978c | 701 | break; |
| shundai | 5:1155a15f978c | 702 | } |
| shundai | 5:1155a15f978c | 703 | |
| shundai | 5:1155a15f978c | 704 | |
| shundai | 5:1155a15f978c | 705 | MDCAN.sendDATA[2] = 0; |
| shundai | 5:1155a15f978c | 706 | MDCAN.sendDATA[3] = 0; |
| shundai | 5:1155a15f978c | 707 | MDCAN.sendDATA[4] = 0; |
| shundai | 5:1155a15f978c | 708 | MDCAN.sendDATA[5] = 0; |
| shundai | 5:1155a15f978c | 709 | MDCAN.sendDATA[6] = 0; |
| shundai | 5:1155a15f978c | 710 | MDCAN.sendDATA[7] = 0; |
| shundai | 5:1155a15f978c | 711 | |
| shundai | 4:40a764acc4ec | 712 | //余りを計算して，端数は削除するとCANデータに変換できる |
| shundai | 5:1155a15f978c | 713 | if(!can.write(CANMessage((char)((int)MDCAN.deviceID-10)*10+100, MDCAN.sendDATA, 8, CANData, CANStandard))) |
| shundai | 1:844acc585d3d | 714 | { |
| shundai | 4:40a764acc4ec | 715 | DBG("CAN send error! \r\n"); |
| shundai | 1:844acc585d3d | 716 | } |
| shundai | 5:1155a15f978c | 717 | |
| shundai | 5:1155a15f978c | 718 | else |
| shundai | 5:1155a15f978c | 719 | { |
| shundai | 5:1155a15f978c | 720 | pc.printf("send !!"); |
| shundai | 5:1155a15f978c | 721 | } |
| shundai | 1:844acc585d3d | 722 | |
| shundai | 4:40a764acc4ec | 723 | //__enable_irq(); |
| shundai | 4:40a764acc4ec | 724 | //Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD); //Measure rotation speed every 30ms |
| shundai | 4:40a764acc4ec | 725 | //pc.attach(SerialFunction, Serial::RxIrq); |
| shundai | 4:40a764acc4ec | 726 | //userButton.fall(ButtonFunction); |
| shundai | 4:40a764acc4ec | 727 | //can.attach(CANread, CAN::RxIrq); |
| shundai | 1:844acc585d3d | 728 | } |
| shundai | 1:844acc585d3d | 729 | |
| shundai | 1:844acc585d3d | 730 | void CurrentFunction() |
| shundai | 1:844acc585d3d | 731 | { |
| shundai | 4:40a764acc4ec | 732 | |
| shundai | 1:844acc585d3d | 733 | } |
| shundai | 1:844acc585d3d | 734 | |
| shundai | 2:50e50ee8e08a | 735 | void MotorDrive(int mode, double target) |
| shundai | 4:40a764acc4ec | 736 | { |
| shundai | 4:40a764acc4ec | 737 | //theta feebback(mode 1) |
| shundai | 4:40a764acc4ec | 738 | //omega feedback(mode 2) |
| shundai | 4:40a764acc4ec | 739 | //current feedback(mode 3) |
| shundai | 1:844acc585d3d | 740 | |
| shundai | 1:844acc585d3d | 741 | //static int old_direction; |
| shundai | 5:1155a15f978c | 742 | //static double old_target; |
| shundai | 1:844acc585d3d | 743 | |
| shundai | 1:844acc585d3d | 744 | switch (mode) |
| shundai | 1:844acc585d3d | 745 | { |
| shundai | 1:844acc585d3d | 746 | case THETA_FEEDBACK: |
| shundai | 1:844acc585d3d | 747 | |
| shundai | 5:1155a15f978c | 748 | //DBG("THETA_FEEDBACK!! \r\n"); |
| shundai | 1:844acc585d3d | 749 | |
| shundai | 5:1155a15f978c | 750 | thetaPID.setSetPoint(target); |
| shundai | 5:1155a15f978c | 751 | thetaPID.setProcessValue(Motor.theta); |
| shundai | 1:844acc585d3d | 752 | |
| shundai | 2:50e50ee8e08a | 753 | if(target >= 0) |
| shundai | 1:844acc585d3d | 754 | { |
| shundai | 5:1155a15f978c | 755 | // Friction compensation |
| shundai | 4:40a764acc4ec | 756 | if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止中 |
| shundai | 1:844acc585d3d | 757 | { |
| shundai | 5:1155a15f978c | 758 | //SR = ABLE; |
| shundai | 5:1155a15f978c | 759 | //PHASE = (float)CW; |
| shundai | 5:1155a15f978c | 760 | //PWM1 = 0.9f; |
| shundai | 5:1155a15f978c | 761 | //PWM2 = 1.0f; |
| shundai | 1:844acc585d3d | 762 | } |
| shundai | 2:50e50ee8e08a | 763 | |
| shundai | 4:40a764acc4ec | 764 | if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上 |
| shundai | 1:844acc585d3d | 765 | { |
| shundai | 5:1155a15f978c | 766 | MotorDrive(OMEGA_FEEDBACK, thetaPID.compute()); |
| shundai | 1:844acc585d3d | 767 | } |
| shundai | 1:844acc585d3d | 768 | |
| shundai | 3:141dc1666df2 | 769 | else //誤差の範囲内ならモータのPWMを停止させる |
| shundai | 3:141dc1666df2 | 770 | { |
| shundai | 3:141dc1666df2 | 771 | MotorDrive(OMEGA_FEEDBACK, 0.0); |
| shundai | 4:40a764acc4ec | 772 | PWM1 = 0.0f; |
| shundai | 4:40a764acc4ec | 773 | PWM2 = 1.0f; |
| shundai | 4:40a764acc4ec | 774 | } |
| shundai | 1:844acc585d3d | 775 | } |
| shundai | 1:844acc585d3d | 776 | |
| shundai | 1:844acc585d3d | 777 | else |
| shundai | 5:1155a15f978c | 778 | { |
| shundai | 5:1155a15f978c | 779 | // Friction compensation |
| shundai | 4:40a764acc4ec | 780 | if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止時 |
| shundai | 1:844acc585d3d | 781 | { |
| shundai | 5:1155a15f978c | 782 | //SR = ABLE; |
| shundai | 5:1155a15f978c | 783 | //PHASE = (float)CCW; |
| shundai | 5:1155a15f978c | 784 | //PWM1 = 0.9f; |
| shundai | 5:1155a15f978c | 785 | //PWM2 = 1.0f; |
| shundai | 1:844acc585d3d | 786 | } |
| shundai | 1:844acc585d3d | 787 | |
| shundai | 4:40a764acc4ec | 788 | if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上 |
| shundai | 1:844acc585d3d | 789 | { |
| shundai | 5:1155a15f978c | 790 | MotorDrive(OMEGA_FEEDBACK, thetaPID.compute()); |
| shundai | 1:844acc585d3d | 791 | } |
| shundai | 1:844acc585d3d | 792 | |
| shundai | 3:141dc1666df2 | 793 | else //誤差の範囲内ならモータのPWMを停止させる |
| shundai | 3:141dc1666df2 | 794 | { |
| shundai | 4:40a764acc4ec | 795 | MotorDrive(OMEGA_FEEDBACK, 0.0); |
| shundai | 3:141dc1666df2 | 796 | PWM1 = 0.0f; |
| shundai | 4:40a764acc4ec | 797 | PWM2 = 1.0f; |
| shundai | 3:141dc1666df2 | 798 | } |
| shundai | 1:844acc585d3d | 799 | } |
| shundai | 1:844acc585d3d | 800 | |
| shundai | 1:844acc585d3d | 801 | break; |
| shundai | 1:844acc585d3d | 802 | |
| shundai | 1:844acc585d3d | 803 | case OMEGA_FEEDBACK: |
| shundai | 1:844acc585d3d | 804 | |
| shundai | 5:1155a15f978c | 805 | //DBG("OMEGA_FEEDBACK "); |
| shundai | 2:50e50ee8e08a | 806 | |
| shundai | 2:50e50ee8e08a | 807 | if(target >= 0.0f) |
| shundai | 2:50e50ee8e08a | 808 | { |
| shundai | 5:1155a15f978c | 809 | omegaPID.setSetPoint(abs(target)); //目標値入力 |
| shundai | 5:1155a15f978c | 810 | omegaPID.setProcessValue(abs(Motor.omega));//センサー入力(絶対値いる？) |
| shundai | 1:844acc585d3d | 811 | //ここにtargetの正負によって回転方向を帰るプログラムを書く |
| shundai | 1:844acc585d3d | 812 | SR = ABLE; |
| shundai | 2:50e50ee8e08a | 813 | PHASE = (float)CW; |
| shundai | 5:1155a15f978c | 814 | PWM1 = omegaPID.compute(); |
| shundai | 1:844acc585d3d | 815 | PWM2 = 1.0f; |
| shundai | 1:844acc585d3d | 816 | |
| shundai | 4:40a764acc4ec | 817 | //DBG("%f \r\n", (float)direction); |
| shundai | 2:50e50ee8e08a | 818 | } |
| shundai | 2:50e50ee8e08a | 819 | |
| shundai | 2:50e50ee8e08a | 820 | else |
| shundai | 2:50e50ee8e08a | 821 | { |
| shundai | 5:1155a15f978c | 822 | omegaPID.setSetPoint(abs(target)); |
| shundai | 5:1155a15f978c | 823 | omegaPID.setProcessValue(abs(Motor.omega)); |
| shundai | 5:1155a15f978c | 824 | //ここにtargetの正負によって回転方向を帰るプログラムを書く |
| shundai | 2:50e50ee8e08a | 825 | SR = ABLE; |
| shundai | 2:50e50ee8e08a | 826 | PHASE = (float)CCW; |
| shundai | 5:1155a15f978c | 827 | PWM1 = omegaPID.compute(); |
| shundai | 2:50e50ee8e08a | 828 | PWM2 = 1.0f; |
| shundai | 2:50e50ee8e08a | 829 | } |
| shundai | 2:50e50ee8e08a | 830 | |
| shundai | 1:844acc585d3d | 831 | break; |
| shundai | 1:844acc585d3d | 832 | |
| shundai | 1:844acc585d3d | 833 | case CURRENT_FEEDBACK: |
| shundai | 1:844acc585d3d | 834 | |
| shundai | 4:40a764acc4ec | 835 | DBG("CURRENT_FEEDBACK UNSELECTABLE!! \r\n"); |
| shundai | 1:844acc585d3d | 836 | |
| shundai | 1:844acc585d3d | 837 | break; |
| shundai | 1:844acc585d3d | 838 | |
| shundai | 1:844acc585d3d | 839 | default: |
| shundai | 1:844acc585d3d | 840 | |
| shundai | 4:40a764acc4ec | 841 | DBG("Function:MotorDriver mode error! \r\n"); |
| shundai | 1:844acc585d3d | 842 | |
| shundai | 1:844acc585d3d | 843 | break; |
| shundai | 1:844acc585d3d | 844 | } |
| shundai | 1:844acc585d3d | 845 | |
| shundai | 1:844acc585d3d | 846 | //old_direction = direction; |
| shundai | 5:1155a15f978c | 847 | //old_target = target; |
| shundai | 5:1155a15f978c | 848 | } |