shundai miyawaki / Mbed 2 deprecated MotorDriver_ver4-1

モータードライバーver4.1 角度フィードバック（クーロン摩擦補償） 角速度フィードバック

Dependencies:   mbed QEI PID DBG

MotorDriver_ver4-1.cpp@3:141dc1666df2, 2019-07-08 (annotated)

Committer:
shundai
Date:
Mon Jul 08 16:44:35 2019 +0000
Revision:
3:141dc1666df2
Parent:
2:50e50ee8e08a
Child:
4:40a764acc4ec
Child:
6:cc51f9e7ef18
pre complete 2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shundai 1:844acc585d3d 1 /*
shundai 1:844acc585d3d 2 MotorDriver ver4.1 farmware
shundai 1:844acc585d3d 3 author : shundai miyawaki
shundai 1:844acc585d3d 4 2019/06/14 ~
shundai 1:844acc585d3d 5 */
shundai 1:844acc585d3d 6
shundai 1:844acc585d3d 7 #if !defined(TARGET_STM32F303K8)
shundai 1:844acc585d3d 8 #error This code is only for NUCLEO-F303K8 or STM32F303K8T6. Reselect a Platform.
shundai 1:844acc585d3d 9 #endif
shundai 1:844acc585d3d 10
shundai 1:844acc585d3d 11 #define DEBUG //debug ON
shundai 1:844acc585d3d 12 //#undef DEBUG //debug OFF
shundai 1:844acc585d3d 13
shundai 1:844acc585d3d 14
shundai 1:844acc585d3d 15 #include "dbg.h"
shundai 1:844acc585d3d 16 #include "mbed.h"
shundai 1:844acc585d3d 17 //#include <CANformat.h>
shundai 1:844acc585d3d 18
shundai 1:844acc585d3d 19
shundai 1:844acc585d3d 20
shundai 1:844acc585d3d 21
shundai 1:844acc585d3d 22 #include "PID.h"
shundai 1:844acc585d3d 23 #include "QEI.h"
shundai 1:844acc585d3d 24
shundai 2:50e50ee8e08a 25 #define Kp2 1.3f
shundai 1:844acc585d3d 26 #define Ki2 0.0f
shundai 1:844acc585d3d 27 #define Kd2 0.002f
shundai 1:844acc585d3d 28
shundai 1:844acc585d3d 29 #define Kp 0.4f
shundai 1:844acc585d3d 30 #define Ki 0.8f
shundai 1:844acc585d3d 31 #define Kd 0.0f
shundai 1:844acc585d3d 32
shundai 1:844acc585d3d 33 #define THETA_FEEDBACK 0
shundai 1:844acc585d3d 34 #define OMEGA_FEEDBACK 1
shundai 1:844acc585d3d 35 #define CURRENT_FEEDBACK 2
shundai 1:844acc585d3d 36
shundai 1:844acc585d3d 37 //#define hostID 0
shundai 1:844acc585d3d 38
shundai 1:844acc585d3d 39 #define SERIAL_BAUDLATE 115200
shundai 1:844acc585d3d 40 #define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
shundai 1:844acc585d3d 41 #define CURRENT_SENSOR_OFFSET_COUNT 1000
shundai 1:844acc585d3d 42
shundai 1:844acc585d3d 43 #define M_PI 3.141593f
shundai 1:844acc585d3d 44
shundai 1:844acc585d3d 45 #define ABLE 0
shundai 1:844acc585d3d 46 #define DISABLE 1
shundai 1:844acc585d3d 47
shundai 2:50e50ee8e08a 48 #define CW 1
shundai 2:50e50ee8e08a 49 #define CCW 0
shundai 1:844acc585d3d 50 #define MOTOR_FREUENCY 8000 //[Hz]
shundai 1:844acc585d3d 51
shundai 1:844acc585d3d 52 #define MOTOR_TARGET_OMEGA_THRESHOLD 0.5f //[deg/s]
shundai 1:844acc585d3d 53 #define MOTOR_TARGET_THETA_THRESHOLD 0.5f //[rad]
shundai 1:844acc585d3d 54 #define MOTOR_TARGET_CURRENT_THRESHOLD 0.1f //[A]
shundai 1:844acc585d3d 55
shundai 1:844acc585d3d 56 #define uniqueSerialAdd_kl_freescale (unsigned long *)0x40048058
shundai 1:844acc585d3d 57 #define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8
shundai 1:844acc585d3d 58 #define uniqueSerialAddr uniqueSerialAddr_stm32
shundai 1:844acc585d3d 59
shundai 1:844acc585d3d 60 //Controll Period
shundai 1:844acc585d3d 61 #define PID_CONTROLL_PERIOD 10 //[ms]
shundai 1:844acc585d3d 62 #define ENCODER_CONTROLL_PERIOD 8 //[ms]
shundai 1:844acc585d3d 63
shundai 1:844acc585d3d 64 //current sensor define
shundai 1:844acc585d3d 65 #define CURRENT_SENSOR_VCC 3.3 //[V]
shundai 1:844acc585d3d 66 #define CURRENT_SENSOR_RANGE 105 //[mv/A]
shundai 1:844acc585d3d 67
shundai 1:844acc585d3d 68
shundai 1:844acc585d3d 69 /*
shundai 1:844acc585d3d 70 readしたらCAN.readDATAを0にする
shundai 1:844acc585d3d 71 read時は割込み禁止で，CAN timeoutの割込みで0のままなら，すべて停止
shundai 1:844acc585d3d 72 */
shundai 1:844acc585d3d 73 /*
shundai 1:844acc585d3d 74 #define CAN_TIMEOUT 3000 //[ms]
shundai 1:844acc585d3d 75 #define ENCODER_PPR 100.0f
shundai 1:844acc585d3d 76 */
shundai 1:844acc585d3d 77
shundai 1:844acc585d3d 78
shundai 1:844acc585d3d 79
shundai 1:844acc585d3d 80 //standerd define
shundai 1:844acc585d3d 81 Serial pc(PB_6, PB_7, SERIAL_BAUDLATE);
shundai 1:844acc585d3d 82 DigitalOut myled(LED1); //PB_3
shundai 1:844acc585d3d 83 DigitalIn userSW(PB_5);
shundai 1:844acc585d3d 84
shundai 1:844acc585d3d 85 //motor define
shundai 1:844acc585d3d 86 PwmOut PWM1(PA_4);
shundai 1:844acc585d3d 87 PwmOut PWM2(PA_6);
shundai 1:844acc585d3d 88 PwmOut PHASE(PB_0);
shundai 1:844acc585d3d 89 DigitalOut SR(PA_7);
shundai 1:844acc585d3d 90 PID motor(Kp, Ki, Kd, PID_CONTROLL_PERIOD*0.001f);
shundai 1:844acc585d3d 91 PID motor2(Kp2, Ki2, Kd2, PID_CONTROLL_PERIOD*0.001f);
shundai 1:844acc585d3d 92 QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
shundai 1:844acc585d3d 93 AnalogIn Current(PB_1);
shundai 1:844acc585d3d 94
shundai 1:844acc585d3d 95 //CAN define
shundai 1:844acc585d3d 96 CAN can(PA_11, PA_12); //RD(PA_11,D10),TD(PA_12,D2)
shundai 1:844acc585d3d 97 CANMessage msg;
shundai 1:844acc585d3d 98
shundai 1:844acc585d3d 99 struct CANdata
shundai 1:844acc585d3d 100 {
shundai 1:844acc585d3d 101 //int timeout;
shundai 1:844acc585d3d 102 //int deviceID;
shundai 1:844acc585d3d 103 char recvDATA[8];
shundai 1:844acc585d3d 104 char sendDATA[8];
shundai 1:844acc585d3d 105 };
shundai 1:844acc585d3d 106
shundai 1:844acc585d3d 107 struct CANdata MDCAN;
shundai 1:844acc585d3d 108
shundai 1:844acc585d3d 109
shundai 1:844acc585d3d 110 void CANsend();
shundai 1:844acc585d3d 111 //void CANtimeout();
shundai 1:844acc585d3d 112 //void CANtest();
shundai 1:844acc585d3d 113
shundai 1:844acc585d3d 114
shundai 1:844acc585d3d 115
shundai 2:50e50ee8e08a 116 void MotorDrive(int mode, double target);
shundai 1:844acc585d3d 117
shundai 1:844acc585d3d 118 //void CurrentFunction();
shundai 1:844acc585d3d 119
shundai 1:844acc585d3d 120 InterruptIn userButton(PB_5);
shundai 1:844acc585d3d 121
shundai 1:844acc585d3d 122 Ticker Encoder;
shundai 1:844acc585d3d 123
shundai 1:844acc585d3d 124 Timer ENCODER_timer;
shundai 1:844acc585d3d 125
shundai 1:844acc585d3d 126 struct state
shundai 1:844acc585d3d 127 {
shundai 1:844acc585d3d 128 double target;
shundai 1:844acc585d3d 129 double theta;
shundai 1:844acc585d3d 130 double omega;
shundai 1:844acc585d3d 131 double current;
shundai 1:844acc585d3d 132 };
shundai 1:844acc585d3d 133
shundai 1:844acc585d3d 134 struct state Motor;
shundai 1:844acc585d3d 135
shundai 1:844acc585d3d 136 //Current Sensor setup
shundai 1:844acc585d3d 137 double current_bias = 0.0;
shundai 1:844acc585d3d 138
shundai 1:844acc585d3d 139 void Velocity();
shundai 1:844acc585d3d 140 void ButtonFunction();
shundai 1:844acc585d3d 141 void SerialFunction();
shundai 1:844acc585d3d 142 void CANread();
shundai 1:844acc585d3d 143
shundai 1:844acc585d3d 144
shundai 1:844acc585d3d 145
shundai 1:844acc585d3d 146 int main()
shundai 1:844acc585d3d 147 {
shundai 1:844acc585d3d 148 // MPU setup
shundai 1:844acc585d3d 149 pc.printf("MotorDriver_ver4.1 start! \r\n");
shundai 1:844acc585d3d 150 pc.printf("/** MPU INFO **/ \r\n");
shundai 1:844acc585d3d 151 pc.printf("Target PlatForm: STM32F303K8T6 \r\n");
shundai 1:844acc585d3d 152 pc.printf("System Frequency: %d Hz \r\n", SystemCoreClock);
shundai 1:844acc585d3d 153 pc.printf("Program File: %s \r\n", __FILE__);
shundai 1:844acc585d3d 154 pc.printf("Compile Date: %s \r\n", __DATE__);
shundai 1:844acc585d3d 155 pc.printf("Compile Time: %s \r\n", __TIME__);
shundai 1:844acc585d3d 156
shundai 1:844acc585d3d 157 //Unique ID starting addresses for most of STM32 microcontrollers.
shundai 1:844acc585d3d 158 unsigned long *id = (unsigned long *)0x1FFFF7AC;
shundai 1:844acc585d3d 159
shundai 1:844acc585d3d 160 pc.printf("%d, %d, %d \r\n", id[0], id[1], id[2]);
shundai 1:844acc585d3d 161
shundai 1:844acc585d3d 162
shundai 1:844acc585d3d 163 myled = 1;
shundai 1:844acc585d3d 164 wait(1.0);
shundai 1:844acc585d3d 165 myled = 0;
shundai 1:844acc585d3d 166
shundai 1:844acc585d3d 167 //load Flash Data
shundai 1:844acc585d3d 168 int deviceID = 10;
shundai 1:844acc585d3d 169
shundai 1:844acc585d3d 170 // CAN setup
shundai 1:844acc585d3d 171 can.frequency(1000000); //ROS default bps:100kbps -> 500kbps
shundai 1:844acc585d3d 172 //can.filter(deviceID, 0b11111111, CANAny, 0);
shundai 1:844acc585d3d 173 can.reset(); //delete receive buffer
shundai 1:844acc585d3d 174 //can.monitor();
shundai 1:844acc585d3d 175 //can.mode();
shundai 1:844acc585d3d 176
shundai 1:844acc585d3d 177 // motor signal setup
shundai 1:844acc585d3d 178 PWM1.period(1.0f/MOTOR_FREUENCY);
shundai 1:844acc585d3d 179 PWM2.period(1.0f/MOTOR_FREUENCY);
shundai 1:844acc585d3d 180 PHASE.period(1.0f/MOTOR_FREUENCY);
shundai 1:844acc585d3d 181
shundai 1:844acc585d3d 182 PWM1 = 0.0f;
shundai 1:844acc585d3d 183 PWM2 = 0.0f;
shundai 1:844acc585d3d 184 PHASE = 0.0f;
shundai 1:844acc585d3d 185 SR = 0;
shundai 1:844acc585d3d 186
shundai 1:844acc585d3d 187 // PID omega setup
shundai 1:844acc585d3d 188 motor.setInputLimits(0.0f, 3.68f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
shundai 1:844acc585d3d 189 motor.setOutputLimits(0.0f, 1.0f);
shundai 1:844acc585d3d 190 motor.setBias(0.0);
shundai 1:844acc585d3d 191 motor.setMode(AUTO_MODE);
shundai 1:844acc585d3d 192 //motor.setSetPoint(1.0f); //PID target value
shundai 1:844acc585d3d 193
shundai 1:844acc585d3d 194 // PID position setup
shundai 1:844acc585d3d 195 motor2.setInputLimits(-360.0f, 360.0f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
shundai 1:844acc585d3d 196 motor2.setOutputLimits(-3.68f, 3.68f);
shundai 1:844acc585d3d 197 motor2.setBias(0.0);
shundai 1:844acc585d3d 198 motor2.setMode(AUTO_MODE);
shundai 1:844acc585d3d 199 //motor.setSetPoint(1.0f); //PID target value
shundai 1:844acc585d3d 200
shundai 1:844acc585d3d 201 // current sensor setup
shundai 1:844acc585d3d 202 for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++)
shundai 1:844acc585d3d 203 {
shundai 1:844acc585d3d 204 current_bias += Current.read();
shundai 1:844acc585d3d 205 }
shundai 1:844acc585d3d 206
shundai 1:844acc585d3d 207 current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT;
shundai 1:844acc585d3d 208 pc.printf("current_bias_average: %f \r\n", current_bias);
shundai 1:844acc585d3d 209
shundai 1:844acc585d3d 210
shundai 1:844acc585d3d 211
shundai 1:844acc585d3d 212 // interrupt start function
shundai 1:844acc585d3d 213 userButton.mode(PullUp);
shundai 1:844acc585d3d 214 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 215 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 216 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 217 ENCODER_timer.start();
shundai 1:844acc585d3d 218 Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
shundai 1:844acc585d3d 219 //NVIC_SetPriority(TIMER3_IRQn, 1);
shundai 1:844acc585d3d 220
shundai 2:50e50ee8e08a 221 Motor.target = 250.0f; //[deg]
shundai 1:844acc585d3d 222 //Motor.target = 1.0f; //[rps]
shundai 1:844acc585d3d 223
shundai 1:844acc585d3d 224 while(1)
shundai 1:844acc585d3d 225 {
shundai 2:50e50ee8e08a 226 //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
shundai 1:844acc585d3d 227
shundai 2:50e50ee8e08a 228 //pc.printf("motor2.compute : %6.3f motor.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", motor2.compute(), motor.compute(), Motor.theta, Motor.omega);
shundai 2:50e50ee8e08a 229 pc.printf("theta:%6.3f omega:%6.3f\r\n", Motor.theta, Motor.omega);
shundai 1:844acc585d3d 230
shundai 2:50e50ee8e08a 231 MotorDrive(THETA_FEEDBACK, Motor.target);
shundai 2:50e50ee8e08a 232 //MotorDrive(OMEGA_FEEDBACK, Motor.target);
shundai 2:50e50ee8e08a 233
shundai 1:844acc585d3d 234 //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());
shundai 1:844acc585d3d 235 //CurrentFunction();
shundai 1:844acc585d3d 236 }
shundai 1:844acc585d3d 237 }
shundai 1:844acc585d3d 238
shundai 1:844acc585d3d 239 void SerialFunction()
shundai 1:844acc585d3d 240 {
shundai 1:844acc585d3d 241 if(pc.readable() == 1)
shundai 1:844acc585d3d 242 {
shundai 1:844acc585d3d 243 char data = pc.getc();
shundai 1:844acc585d3d 244 pc.printf("get data:%c \r\n", data);
shundai 1:844acc585d3d 245
shundai 1:844acc585d3d 246
shundai 1:844acc585d3d 247 switch (data)
shundai 1:844acc585d3d 248 {
shundai 1:844acc585d3d 249 case 'q':
shundai 1:844acc585d3d 250
shundai 1:844acc585d3d 251 //motor.setSetPoint(1.5f); //PID target value
shundai 1:844acc585d3d 252
shundai 1:844acc585d3d 253 Motor.target += 0.5f;
shundai 1:844acc585d3d 254 pc.printf("get 'q' \r\n");
shundai 1:844acc585d3d 255
shundai 1:844acc585d3d 256 break;
shundai 1:844acc585d3d 257
shundai 1:844acc585d3d 258 case 'a':
shundai 1:844acc585d3d 259
shundai 1:844acc585d3d 260 //motor.setSetPoint(0.5f); //PID target value
shundai 1:844acc585d3d 261
shundai 1:844acc585d3d 262 Motor.target -= 0.5f;
shundai 1:844acc585d3d 263 pc.printf("get 'a' \r\n");
shundai 1:844acc585d3d 264
shundai 1:844acc585d3d 265 break;
shundai 2:50e50ee8e08a 266
shundai 2:50e50ee8e08a 267 case 'p':
shundai 2:50e50ee8e08a 268
shundai 2:50e50ee8e08a 269 //motor.setSetPoint(1.5f); //PID target value
shundai 2:50e50ee8e08a 270
shundai 2:50e50ee8e08a 271 Motor.target += 30.0f;
shundai 2:50e50ee8e08a 272 pc.printf("get 'p' \r\n");
shundai 2:50e50ee8e08a 273
shundai 2:50e50ee8e08a 274 break;
shundai 2:50e50ee8e08a 275
shundai 2:50e50ee8e08a 276 case 'l':
shundai 2:50e50ee8e08a 277
shundai 2:50e50ee8e08a 278 //motor.setSetPoint(0.5f); //PID target value
shundai 2:50e50ee8e08a 279
shundai 2:50e50ee8e08a 280 Motor.target -= 30.0f;
shundai 2:50e50ee8e08a 281 pc.printf("get 'l' \r\n");
shundai 2:50e50ee8e08a 282
shundai 2:50e50ee8e08a 283 break;
shundai 2:50e50ee8e08a 284
shundai 1:844acc585d3d 285
shundai 1:844acc585d3d 286 case 'r':
shundai 1:844acc585d3d 287
shundai 1:844acc585d3d 288
shundai 1:844acc585d3d 289
shundai 1:844acc585d3d 290 pc.printf("\r\nSystem Reset! \r\n");
shundai 1:844acc585d3d 291 NVIC_SystemReset();
shundai 1:844acc585d3d 292
shundai 1:844acc585d3d 293 break;
shundai 1:844acc585d3d 294
shundai 1:844acc585d3d 295 default:
shundai 1:844acc585d3d 296
shundai 1:844acc585d3d 297 pc.printf("default \r\n");
shundai 1:844acc585d3d 298
shundai 1:844acc585d3d 299 break;
shundai 1:844acc585d3d 300 }
shundai 1:844acc585d3d 301 }
shundai 1:844acc585d3d 302 }
shundai 1:844acc585d3d 303
shundai 1:844acc585d3d 304 void ButtonFunction()
shundai 1:844acc585d3d 305 {
shundai 1:844acc585d3d 306 motor.setProcessValue(0); //target value = 0
shundai 1:844acc585d3d 307 //int cmd = 0;
shundai 1:844acc585d3d 308
shundai 1:844acc585d3d 309 pc.printf("setup open \r\n");
shundai 1:844acc585d3d 310
shundai 1:844acc585d3d 311 pc.printf("1:change the CAN device ID \r\n");
shundai 1:844acc585d3d 312 pc.printf("2:change the CAN device ID \r\n");
shundai 1:844acc585d3d 313
shundai 1:844acc585d3d 314 //CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
shundai 1:844acc585d3d 315
shundai 1:844acc585d3d 316 //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
shundai 1:844acc585d3d 317
shundai 1:844acc585d3d 318 while(1)
shundai 1:844acc585d3d 319 {
shundai 1:844acc585d3d 320 if(!pc.readable())
shundai 1:844acc585d3d 321 {
shundai 1:844acc585d3d 322 // pc.scanf("%d", cmd);
shundai 1:844acc585d3d 323 }
shundai 1:844acc585d3d 324
shundai 1:844acc585d3d 325 else
shundai 1:844acc585d3d 326 {
shundai 1:844acc585d3d 327 pc.printf("old device ID:\r\n");
shundai 1:844acc585d3d 328 pc.printf("new device ID:\r\n");
shundai 1:844acc585d3d 329
shundai 1:844acc585d3d 330 while(1)
shundai 1:844acc585d3d 331 {
shundai 1:844acc585d3d 332
shundai 1:844acc585d3d 333 }
shundai 1:844acc585d3d 334 }
shundai 1:844acc585d3d 335 }
shundai 1:844acc585d3d 336 }
shundai 1:844acc585d3d 337
shundai 1:844acc585d3d 338 void CANread()
shundai 1:844acc585d3d 339 {
shundai 1:844acc585d3d 340 __disable_irq(); //All Interrupt disabled
shundai 1:844acc585d3d 341 Encoder.detach();
shundai 1:844acc585d3d 342
shundai 1:844acc585d3d 343
shundai 1:844acc585d3d 344 if(can.read(msg) != 0)
shundai 1:844acc585d3d 345 {
shundai 1:844acc585d3d 346 pc.printf("CAN recieve \r\n");
shundai 1:844acc585d3d 347 myled = !myled;
shundai 1:844acc585d3d 348
shundai 1:844acc585d3d 349 for(int i = 0 ; i < 8 ; i++)
shundai 1:844acc585d3d 350 {
shundai 1:844acc585d3d 351 MDCAN.recvDATA[i] = msg.data[i];
shundai 1:844acc585d3d 352 pc.printf("[%c] ", MDCAN.recvDATA[i]);
shundai 1:844acc585d3d 353 }
shundai 1:844acc585d3d 354
shundai 1:844acc585d3d 355 pc.printf("\r\n");
shundai 1:844acc585d3d 356
shundai 1:844acc585d3d 357 switch(MDCAN.recvDATA[1]) //check PID
shundai 1:844acc585d3d 358 {
shundai 1:844acc585d3d 359 case 0: //MD data send
shundai 1:844acc585d3d 360
shundai 1:844acc585d3d 361 MDCAN.sendDATA[0] = 1; //PID
shundai 1:844acc585d3d 362 MDCAN.sendDATA[1] = 0; //dataA
shundai 1:844acc585d3d 363 MDCAN.sendDATA[2] = 0; //dataB
shundai 1:844acc585d3d 364 MDCAN.sendDATA[3] = 0; //dataC
shundai 1:844acc585d3d 365 MDCAN.sendDATA[4] = 0; //dataD
shundai 1:844acc585d3d 366 MDCAN.sendDATA[5] = 0; //dataE
shundai 1:844acc585d3d 367 MDCAN.sendDATA[6] = 0; //dataF
shundai 1:844acc585d3d 368 MDCAN.sendDATA[7] = 15;//dataG
shundai 1:844acc585d3d 369
shundai 1:844acc585d3d 370 //CANsend(hostID);
shundai 1:844acc585d3d 371
shundai 1:844acc585d3d 372 //pc.printf("reply to host(PID=%d)\r\n",PID);
shundai 1:844acc585d3d 373
shundai 1:844acc585d3d 374 break;
shundai 1:844acc585d3d 375
shundai 1:844acc585d3d 376 case 9: //MD drive
shundai 1:844acc585d3d 377
shundai 1:844acc585d3d 378 ///MD4.Drive();
shundai 1:844acc585d3d 379
shundai 1:844acc585d3d 380 break;
shundai 1:844acc585d3d 381
shundai 1:844acc585d3d 382 default:
shundai 1:844acc585d3d 383
shundai 1:844acc585d3d 384 //CAN.recvDATA[1] = 0b00000000;
shundai 1:844acc585d3d 385
shundai 1:844acc585d3d 386 break;
shundai 1:844acc585d3d 387 }
shundai 1:844acc585d3d 388 }
shundai 1:844acc585d3d 389
shundai 1:844acc585d3d 390 motor.setSetPoint((double)MDCAN.recvDATA[1]); //motor target value velocity
shundai 1:844acc585d3d 391 __enable_irq();
shundai 1:844acc585d3d 392 Encoder.attach(Velocity, 0.01); //Measure rotation speed every 10ms
shundai 1:844acc585d3d 393 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 394 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 395 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 396 }
shundai 1:844acc585d3d 397
shundai 1:844acc585d3d 398 void Velocity()
shundai 1:844acc585d3d 399 {
shundai 1:844acc585d3d 400 static int pulse; //old pulse
shundai 1:844acc585d3d 401
shundai 1:844acc585d3d 402 Motor.omega = ((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read();
shundai 1:844acc585d3d 403 pulse = encoder.getPulses(); //pulse update
shundai 1:844acc585d3d 404
shundai 1:844acc585d3d 405 Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f;
shundai 1:844acc585d3d 406
shundai 1:844acc585d3d 407 ENCODER_timer.reset(); //timer reset:t=0s
shundai 1:844acc585d3d 408
shundai 1:844acc585d3d 409 //pc.printf("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
shundai 1:844acc585d3d 410 }
shundai 1:844acc585d3d 411
shundai 1:844acc585d3d 412 void CANsend(int id)
shundai 1:844acc585d3d 413 {
shundai 1:844acc585d3d 414 __disable_irq();
shundai 1:844acc585d3d 415 Encoder.detach();
shundai 1:844acc585d3d 416
shundai 1:844acc585d3d 417 if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended)))
shundai 1:844acc585d3d 418 {
shundai 1:844acc585d3d 419 pc.printf("CAN send error! \r\n");
shundai 1:844acc585d3d 420 }
shundai 1:844acc585d3d 421
shundai 1:844acc585d3d 422 __enable_irq();
shundai 1:844acc585d3d 423 Encoder.attach(Velocity, 0.03); //Measure rotation speed every 30ms
shundai 1:844acc585d3d 424 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 425 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 426 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 427 }
shundai 1:844acc585d3d 428
shundai 1:844acc585d3d 429 /*
shundai 1:844acc585d3d 430 void CANtimeout()
shundai 1:844acc585d3d 431 {
shundai 1:844acc585d3d 432 if(MDCAN.recvDATA[2] == 0)
shundai 1:844acc585d3d 433 {
shundai 1:844acc585d3d 434 pc.printf("CAN CONNECTION TIMEOUT \r\n");
shundai 1:844acc585d3d 435 //MotorDrive();
shundai 1:844acc585d3d 436 }
shundai 1:844acc585d3d 437 }
shundai 1:844acc585d3d 438
shundai 1:844acc585d3d 439 void CurrentFunction()
shundai 1:844acc585d3d 440 {
shundai 1:844acc585d3d 441 static int noneDrive_count;
shundai 1:844acc585d3d 442
shundai 1:844acc585d3d 443
shundai 1:844acc585d3d 444 pc.printf("current(raw) : %f \r\n",Current.read());
shundai 1:844acc585d3d 445
shundai 1:844acc585d3d 446 Motor.current = (Current.read() - current_bias);
shundai 1:844acc585d3d 447
shundai 1:844acc585d3d 448 if(noneDrive_count >= 2)
shundai 1:844acc585d3d 449 {
shundai 1:844acc585d3d 450 CANsend(); //Power capacity lack. check power supply output current
shundai 1:844acc585d3d 451 }
shundai 1:844acc585d3d 452
shundai 1:844acc585d3d 453 //if(MAX_LIMIT_CURRENT)
shundai 1:844acc585d3d 454
shundai 1:844acc585d3d 455 }
shundai 1:844acc585d3d 456 */
shundai 1:844acc585d3d 457
shundai 2:50e50ee8e08a 458 void MotorDrive(int mode, double target)
shundai 1:844acc585d3d 459 {
shundai 1:844acc585d3d 460 //theta feebback(mode 0)
shundai 1:844acc585d3d 461 //omega feedback(mode 1)
shundai 1:844acc585d3d 462 //current feedback(mode 2)
shundai 1:844acc585d3d 463
shundai 1:844acc585d3d 464 //static int old_direction;
shundai 1:844acc585d3d 465 static double old_target;
shundai 1:844acc585d3d 466
shundai 1:844acc585d3d 467 switch (mode)
shundai 1:844acc585d3d 468 {
shundai 1:844acc585d3d 469 case THETA_FEEDBACK:
shundai 1:844acc585d3d 470
shundai 2:50e50ee8e08a 471 //pc.printf("THETA_FEEDBACK!! \r\n");
shundai 1:844acc585d3d 472
shundai 1:844acc585d3d 473 motor2.setSetPoint(target);
shundai 1:844acc585d3d 474 motor2.setProcessValue(Motor.theta);
shundai 1:844acc585d3d 475
shundai 2:50e50ee8e08a 476 if(target >= 0)
shundai 1:844acc585d3d 477 {
shundai 3:141dc1666df2 478 if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止中
shundai 1:844acc585d3d 479 {
shundai 1:844acc585d3d 480 SR = ABLE;
shundai 2:50e50ee8e08a 481 PHASE = (float)CW;
shundai 1:844acc585d3d 482 }
shundai 2:50e50ee8e08a 483
shundai 3:141dc1666df2 484 if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
shundai 1:844acc585d3d 485 {
shundai 2:50e50ee8e08a 486 MotorDrive(OMEGA_FEEDBACK, motor2.compute());
shundai 1:844acc585d3d 487 }
shundai 1:844acc585d3d 488
shundai 3:141dc1666df2 489 else //誤差の範囲内ならモータのPWMを停止させる
shundai 3:141dc1666df2 490 {
shundai 3:141dc1666df2 491 MotorDrive(OMEGA_FEEDBACK, 0.0);
shundai 3:141dc1666df2 492 }
shundai 3:141dc1666df2 493
shundai 1:844acc585d3d 494 //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
shundai 1:844acc585d3d 495 }
shundai 1:844acc585d3d 496
shundai 1:844acc585d3d 497 else
shundai 1:844acc585d3d 498 {
shundai 1:844acc585d3d 499
shundai 3:141dc1666df2 500 if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止時
shundai 1:844acc585d3d 501 {
shundai 1:844acc585d3d 502 SR = ABLE;
shundai 2:50e50ee8e08a 503 PHASE = (float)CCW;
shundai 3:141dc1666df2 504 PWM1 = 0.3f;
shundai 1:844acc585d3d 505 PWM2 = 1.0f;
shundai 1:844acc585d3d 506 }
shundai 1:844acc585d3d 507
shundai 3:141dc1666df2 508 if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
shundai 1:844acc585d3d 509 {
shundai 2:50e50ee8e08a 510 MotorDrive(OMEGA_FEEDBACK, motor2.compute());
shundai 1:844acc585d3d 511 }
shundai 1:844acc585d3d 512
shundai 3:141dc1666df2 513 else //誤差の範囲内ならモータのPWMを停止させる
shundai 3:141dc1666df2 514 {
shundai 3:141dc1666df2 515 MotorDrive(OMEGA_FEEDBACK, 0.0);
shundai 3:141dc1666df2 516 PWM1 = 0.0f;
shundai 3:141dc1666df2 517 PWM2 = 1.0f;
shundai 3:141dc1666df2 518 }
shundai 3:141dc1666df2 519
shundai 1:844acc585d3d 520 //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
shundai 1:844acc585d3d 521 }
shundai 1:844acc585d3d 522
shundai 1:844acc585d3d 523 /*
shundai 1:844acc585d3d 524 SR = ABLE;
shundai 1:844acc585d3d 525 PHASE = (float)direction;
shundai 1:844acc585d3d 526 PWM1 = motor.compute();
shundai 1:844acc585d3d 527 PWM2 = 1.0f;
shundai 1:844acc585d3d 528 */
shundai 1:844acc585d3d 529 break;
shundai 1:844acc585d3d 530
shundai 1:844acc585d3d 531 case OMEGA_FEEDBACK:
shundai 1:844acc585d3d 532
shundai 2:50e50ee8e08a 533 /*
shundai 1:844acc585d3d 534 if(direction != old_direction) //Prevent sudden turn
shundai 1:844acc585d3d 535 {
shundai 1:844acc585d3d 536 motor.setProcessValue(0.0f);
shundai 1:844acc585d3d 537
shundai 1:844acc585d3d 538 if(motor.compute() < 0.05f)
shundai 1:844acc585d3d 539 {
shundai 1:844acc585d3d 540 old_direction = direction;
shundai 1:844acc585d3d 541 }
shundai 1:844acc585d3d 542 }
shundai 2:50e50ee8e08a 543 */
shundai 1:844acc585d3d 544
shundai 2:50e50ee8e08a 545
shundai 2:50e50ee8e08a 546 //pc.printf("OMEGA_FEEDBACK ");
shundai 2:50e50ee8e08a 547
shundai 2:50e50ee8e08a 548 if(target >= 0.0f)
shundai 2:50e50ee8e08a 549 {
shundai 1:844acc585d3d 550 motor.setSetPoint(abs(target));
shundai 1:844acc585d3d 551 motor.setProcessValue(abs(Motor.omega));
shundai 1:844acc585d3d 552 //ここにtargetの正負によって回転方向を帰るプログラムを書く
shundai 1:844acc585d3d 553 SR = ABLE;
shundai 2:50e50ee8e08a 554 PHASE = (float)CW;
shundai 1:844acc585d3d 555 PWM1 = motor.compute();
shundai 1:844acc585d3d 556 PWM2 = 1.0f;
shundai 1:844acc585d3d 557
shundai 2:50e50ee8e08a 558 //pc.printf("%f \r\n", (float)direction);
shundai 2:50e50ee8e08a 559 }
shundai 2:50e50ee8e08a 560
shundai 2:50e50ee8e08a 561 else
shundai 2:50e50ee8e08a 562 {
shundai 2:50e50ee8e08a 563 motor.setSetPoint(abs(target));
shundai 2:50e50ee8e08a 564 motor.setProcessValue(abs(Motor.omega));
shundai 2:50e50ee8e08a 565 //ここにtargetの正負によって回転方向を帰るプログラムを書く
shundai 2:50e50ee8e08a 566 SR = ABLE;
shundai 2:50e50ee8e08a 567 PHASE = (float)CCW;
shundai 2:50e50ee8e08a 568 PWM1 = motor.compute();
shundai 2:50e50ee8e08a 569 PWM2 = 1.0f;
shundai 2:50e50ee8e08a 570 }
shundai 2:50e50ee8e08a 571
shundai 1:844acc585d3d 572 break;
shundai 1:844acc585d3d 573
shundai 1:844acc585d3d 574 case CURRENT_FEEDBACK:
shundai 1:844acc585d3d 575
shundai 1:844acc585d3d 576 pc.printf("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
shundai 1:844acc585d3d 577
shundai 1:844acc585d3d 578 break;
shundai 1:844acc585d3d 579
shundai 1:844acc585d3d 580 default:
shundai 1:844acc585d3d 581
shundai 1:844acc585d3d 582 pc.printf("Function:MotorDriver mode error! \r\n");
shundai 1:844acc585d3d 583
shundai 1:844acc585d3d 584 break;
shundai 1:844acc585d3d 585 }
shundai 1:844acc585d3d 586
shundai 1:844acc585d3d 587
shundai 1:844acc585d3d 588 //motor.setProcessValue(omega);
shundai 1:844acc585d3d 589
shundai 1:844acc585d3d 590 //old_direction = direction;
shundai 1:844acc585d3d 591 old_target = target;
shundai 1:844acc585d3d 592 }
shundai 1:844acc585d3d 593
shundai 1:844acc585d3d 594
shundai 1:844acc585d3d 595 /*
shundai 1:844acc585d3d 596 void CAN2DATA()
shundai 1:844acc585d3d 597
shundai 1:844acc585d3d 598 void DATA2CAN()
shundai 1:844acc585d3d 599 */