shundai miyawaki / Mbed 2 deprecated MotorDriver_ver4-1

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

Dependencies:   mbed QEI PID DBG

MotorDriver_ver4-1.cpp@2:50e50ee8e08a, 2019-07-08 (annotated)

Committer:
shundai
Date:
Mon Jul 08 15:47:40 2019 +0000
Revision:
2:50e50ee8e08a
Parent:
1:844acc585d3d
Child:
3:141dc1666df2
MDver4.1 pre complete;

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 1:844acc585d3d 226
shundai 1:844acc585d3d 227
shundai 2:50e50ee8e08a 228 //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
shundai 1:844acc585d3d 229
shundai 2:50e50ee8e08a 230 //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 231 pc.printf("theta:%6.3f omega:%6.3f\r\n", Motor.theta, Motor.omega);
shundai 1:844acc585d3d 232
shundai 2:50e50ee8e08a 233 MotorDrive(THETA_FEEDBACK, Motor.target);
shundai 2:50e50ee8e08a 234 //MotorDrive(OMEGA_FEEDBACK, Motor.target);
shundai 2:50e50ee8e08a 235
shundai 1:844acc585d3d 236 //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());
shundai 1:844acc585d3d 237 //CurrentFunction();
shundai 1:844acc585d3d 238 }
shundai 1:844acc585d3d 239 }
shundai 1:844acc585d3d 240
shundai 1:844acc585d3d 241 void SerialFunction()
shundai 1:844acc585d3d 242 {
shundai 1:844acc585d3d 243 if(pc.readable() == 1)
shundai 1:844acc585d3d 244 {
shundai 1:844acc585d3d 245 char data = pc.getc();
shundai 1:844acc585d3d 246 pc.printf("get data:%c \r\n", data);
shundai 1:844acc585d3d 247
shundai 1:844acc585d3d 248
shundai 1:844acc585d3d 249 switch (data)
shundai 1:844acc585d3d 250 {
shundai 1:844acc585d3d 251 case 'q':
shundai 1:844acc585d3d 252
shundai 1:844acc585d3d 253 //motor.setSetPoint(1.5f); //PID target value
shundai 1:844acc585d3d 254
shundai 1:844acc585d3d 255 Motor.target += 0.5f;
shundai 1:844acc585d3d 256 pc.printf("get 'q' \r\n");
shundai 1:844acc585d3d 257
shundai 1:844acc585d3d 258 break;
shundai 1:844acc585d3d 259
shundai 1:844acc585d3d 260 case 'a':
shundai 1:844acc585d3d 261
shundai 1:844acc585d3d 262 //motor.setSetPoint(0.5f); //PID target value
shundai 1:844acc585d3d 263
shundai 1:844acc585d3d 264 Motor.target -= 0.5f;
shundai 1:844acc585d3d 265 pc.printf("get 'a' \r\n");
shundai 1:844acc585d3d 266
shundai 1:844acc585d3d 267 break;
shundai 2:50e50ee8e08a 268
shundai 2:50e50ee8e08a 269 case 'p':
shundai 2:50e50ee8e08a 270
shundai 2:50e50ee8e08a 271 //motor.setSetPoint(1.5f); //PID target value
shundai 2:50e50ee8e08a 272
shundai 2:50e50ee8e08a 273 Motor.target += 30.0f;
shundai 2:50e50ee8e08a 274 pc.printf("get 'p' \r\n");
shundai 2:50e50ee8e08a 275
shundai 2:50e50ee8e08a 276 break;
shundai 2:50e50ee8e08a 277
shundai 2:50e50ee8e08a 278 case 'l':
shundai 2:50e50ee8e08a 279
shundai 2:50e50ee8e08a 280 //motor.setSetPoint(0.5f); //PID target value
shundai 2:50e50ee8e08a 281
shundai 2:50e50ee8e08a 282 Motor.target -= 30.0f;
shundai 2:50e50ee8e08a 283 pc.printf("get 'l' \r\n");
shundai 2:50e50ee8e08a 284
shundai 2:50e50ee8e08a 285 break;
shundai 2:50e50ee8e08a 286
shundai 1:844acc585d3d 287
shundai 1:844acc585d3d 288 case 'r':
shundai 1:844acc585d3d 289
shundai 1:844acc585d3d 290
shundai 1:844acc585d3d 291
shundai 1:844acc585d3d 292 pc.printf("\r\nSystem Reset! \r\n");
shundai 1:844acc585d3d 293 NVIC_SystemReset();
shundai 1:844acc585d3d 294
shundai 1:844acc585d3d 295 break;
shundai 1:844acc585d3d 296
shundai 1:844acc585d3d 297 default:
shundai 1:844acc585d3d 298
shundai 1:844acc585d3d 299 pc.printf("default \r\n");
shundai 1:844acc585d3d 300
shundai 1:844acc585d3d 301 break;
shundai 1:844acc585d3d 302 }
shundai 1:844acc585d3d 303 }
shundai 1:844acc585d3d 304 }
shundai 1:844acc585d3d 305
shundai 1:844acc585d3d 306 void ButtonFunction()
shundai 1:844acc585d3d 307 {
shundai 1:844acc585d3d 308 motor.setProcessValue(0); //target value = 0
shundai 1:844acc585d3d 309 //int cmd = 0;
shundai 1:844acc585d3d 310
shundai 1:844acc585d3d 311 pc.printf("setup open \r\n");
shundai 1:844acc585d3d 312
shundai 1:844acc585d3d 313 pc.printf("1:change the CAN device ID \r\n");
shundai 1:844acc585d3d 314 pc.printf("2:change the CAN device ID \r\n");
shundai 1:844acc585d3d 315
shundai 1:844acc585d3d 316 //CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
shundai 1:844acc585d3d 317
shundai 1:844acc585d3d 318 //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
shundai 1:844acc585d3d 319
shundai 1:844acc585d3d 320 while(1)
shundai 1:844acc585d3d 321 {
shundai 1:844acc585d3d 322 if(!pc.readable())
shundai 1:844acc585d3d 323 {
shundai 1:844acc585d3d 324 // pc.scanf("%d", cmd);
shundai 1:844acc585d3d 325 }
shundai 1:844acc585d3d 326
shundai 1:844acc585d3d 327 else
shundai 1:844acc585d3d 328 {
shundai 1:844acc585d3d 329 pc.printf("old device ID:\r\n");
shundai 1:844acc585d3d 330 pc.printf("new device ID:\r\n");
shundai 1:844acc585d3d 331
shundai 1:844acc585d3d 332 while(1)
shundai 1:844acc585d3d 333 {
shundai 1:844acc585d3d 334
shundai 1:844acc585d3d 335 }
shundai 1:844acc585d3d 336 }
shundai 1:844acc585d3d 337 }
shundai 1:844acc585d3d 338 }
shundai 1:844acc585d3d 339
shundai 1:844acc585d3d 340 void CANread()
shundai 1:844acc585d3d 341 {
shundai 1:844acc585d3d 342 __disable_irq(); //All Interrupt disabled
shundai 1:844acc585d3d 343 Encoder.detach();
shundai 1:844acc585d3d 344
shundai 1:844acc585d3d 345
shundai 1:844acc585d3d 346 if(can.read(msg) != 0)
shundai 1:844acc585d3d 347 {
shundai 1:844acc585d3d 348 pc.printf("CAN recieve \r\n");
shundai 1:844acc585d3d 349 myled = !myled;
shundai 1:844acc585d3d 350
shundai 1:844acc585d3d 351 for(int i = 0 ; i < 8 ; i++)
shundai 1:844acc585d3d 352 {
shundai 1:844acc585d3d 353 MDCAN.recvDATA[i] = msg.data[i];
shundai 1:844acc585d3d 354 pc.printf("[%c] ", MDCAN.recvDATA[i]);
shundai 1:844acc585d3d 355 }
shundai 1:844acc585d3d 356
shundai 1:844acc585d3d 357 pc.printf("\r\n");
shundai 1:844acc585d3d 358
shundai 1:844acc585d3d 359 switch(MDCAN.recvDATA[1]) //check PID
shundai 1:844acc585d3d 360 {
shundai 1:844acc585d3d 361 case 0: //MD data send
shundai 1:844acc585d3d 362
shundai 1:844acc585d3d 363 MDCAN.sendDATA[0] = 1; //PID
shundai 1:844acc585d3d 364 MDCAN.sendDATA[1] = 0; //dataA
shundai 1:844acc585d3d 365 MDCAN.sendDATA[2] = 0; //dataB
shundai 1:844acc585d3d 366 MDCAN.sendDATA[3] = 0; //dataC
shundai 1:844acc585d3d 367 MDCAN.sendDATA[4] = 0; //dataD
shundai 1:844acc585d3d 368 MDCAN.sendDATA[5] = 0; //dataE
shundai 1:844acc585d3d 369 MDCAN.sendDATA[6] = 0; //dataF
shundai 1:844acc585d3d 370 MDCAN.sendDATA[7] = 15;//dataG
shundai 1:844acc585d3d 371
shundai 1:844acc585d3d 372 //CANsend(hostID);
shundai 1:844acc585d3d 373
shundai 1:844acc585d3d 374 //pc.printf("reply to host(PID=%d)\r\n",PID);
shundai 1:844acc585d3d 375
shundai 1:844acc585d3d 376 break;
shundai 1:844acc585d3d 377
shundai 1:844acc585d3d 378 case 9: //MD drive
shundai 1:844acc585d3d 379
shundai 1:844acc585d3d 380 ///MD4.Drive();
shundai 1:844acc585d3d 381
shundai 1:844acc585d3d 382 break;
shundai 1:844acc585d3d 383
shundai 1:844acc585d3d 384 default:
shundai 1:844acc585d3d 385
shundai 1:844acc585d3d 386 //CAN.recvDATA[1] = 0b00000000;
shundai 1:844acc585d3d 387
shundai 1:844acc585d3d 388 break;
shundai 1:844acc585d3d 389 }
shundai 1:844acc585d3d 390 }
shundai 1:844acc585d3d 391
shundai 1:844acc585d3d 392 motor.setSetPoint((double)MDCAN.recvDATA[1]); //motor target value velocity
shundai 1:844acc585d3d 393 __enable_irq();
shundai 1:844acc585d3d 394 Encoder.attach(Velocity, 0.01); //Measure rotation speed every 10ms
shundai 1:844acc585d3d 395 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 396 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 397 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 398 }
shundai 1:844acc585d3d 399
shundai 1:844acc585d3d 400 void Velocity()
shundai 1:844acc585d3d 401 {
shundai 1:844acc585d3d 402 static int pulse; //old pulse
shundai 1:844acc585d3d 403
shundai 1:844acc585d3d 404 Motor.omega = ((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read();
shundai 1:844acc585d3d 405 pulse = encoder.getPulses(); //pulse update
shundai 1:844acc585d3d 406
shundai 1:844acc585d3d 407 Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f;
shundai 1:844acc585d3d 408
shundai 1:844acc585d3d 409 ENCODER_timer.reset(); //timer reset:t=0s
shundai 1:844acc585d3d 410
shundai 1:844acc585d3d 411 //pc.printf("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
shundai 1:844acc585d3d 412 }
shundai 1:844acc585d3d 413
shundai 1:844acc585d3d 414 void CANsend(int id)
shundai 1:844acc585d3d 415 {
shundai 1:844acc585d3d 416 __disable_irq();
shundai 1:844acc585d3d 417 Encoder.detach();
shundai 1:844acc585d3d 418
shundai 1:844acc585d3d 419 if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended)))
shundai 1:844acc585d3d 420 {
shundai 1:844acc585d3d 421 pc.printf("CAN send error! \r\n");
shundai 1:844acc585d3d 422 }
shundai 1:844acc585d3d 423
shundai 1:844acc585d3d 424 __enable_irq();
shundai 1:844acc585d3d 425 Encoder.attach(Velocity, 0.03); //Measure rotation speed every 30ms
shundai 1:844acc585d3d 426 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 427 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 428 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 429 }
shundai 1:844acc585d3d 430
shundai 1:844acc585d3d 431 /*
shundai 1:844acc585d3d 432 void CANtimeout()
shundai 1:844acc585d3d 433 {
shundai 1:844acc585d3d 434 if(MDCAN.recvDATA[2] == 0)
shundai 1:844acc585d3d 435 {
shundai 1:844acc585d3d 436 pc.printf("CAN CONNECTION TIMEOUT \r\n");
shundai 1:844acc585d3d 437 //MotorDrive();
shundai 1:844acc585d3d 438 }
shundai 1:844acc585d3d 439 }
shundai 1:844acc585d3d 440
shundai 1:844acc585d3d 441 void CurrentFunction()
shundai 1:844acc585d3d 442 {
shundai 1:844acc585d3d 443 static int noneDrive_count;
shundai 1:844acc585d3d 444
shundai 1:844acc585d3d 445
shundai 1:844acc585d3d 446 pc.printf("current(raw) : %f \r\n",Current.read());
shundai 1:844acc585d3d 447
shundai 1:844acc585d3d 448 Motor.current = (Current.read() - current_bias);
shundai 1:844acc585d3d 449
shundai 1:844acc585d3d 450 if(noneDrive_count >= 2)
shundai 1:844acc585d3d 451 {
shundai 1:844acc585d3d 452 CANsend(); //Power capacity lack. check power supply output current
shundai 1:844acc585d3d 453 }
shundai 1:844acc585d3d 454
shundai 1:844acc585d3d 455 //if(MAX_LIMIT_CURRENT)
shundai 1:844acc585d3d 456
shundai 1:844acc585d3d 457 }
shundai 1:844acc585d3d 458 */
shundai 1:844acc585d3d 459
shundai 2:50e50ee8e08a 460 void MotorDrive(int mode, double target)
shundai 1:844acc585d3d 461 {
shundai 1:844acc585d3d 462 //theta feebback(mode 0)
shundai 1:844acc585d3d 463 //omega feedback(mode 1)
shundai 1:844acc585d3d 464 //current feedback(mode 2)
shundai 1:844acc585d3d 465
shundai 1:844acc585d3d 466 //static int old_direction;
shundai 1:844acc585d3d 467 static double old_target;
shundai 1:844acc585d3d 468
shundai 1:844acc585d3d 469 switch (mode)
shundai 1:844acc585d3d 470 {
shundai 1:844acc585d3d 471 case THETA_FEEDBACK:
shundai 1:844acc585d3d 472
shundai 2:50e50ee8e08a 473 //pc.printf("THETA_FEEDBACK!! \r\n");
shundai 1:844acc585d3d 474
shundai 1:844acc585d3d 475 motor2.setSetPoint(target);
shundai 1:844acc585d3d 476 motor2.setProcessValue(Motor.theta);
shundai 1:844acc585d3d 477
shundai 2:50e50ee8e08a 478 if(target >= 0)
shundai 1:844acc585d3d 479 {
shundai 2:50e50ee8e08a 480 if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
shundai 1:844acc585d3d 481 {
shundai 1:844acc585d3d 482 SR = ABLE;
shundai 2:50e50ee8e08a 483 PHASE = (float)CW;
shundai 1:844acc585d3d 484 PWM1 = 0.4f;
shundai 1:844acc585d3d 485 PWM2 = 1.0f;
shundai 1:844acc585d3d 486 //MotorDrive(CURRENT_FEEDBACK, CCW, 1.2f);
shundai 1:844acc585d3d 487 }
shundai 2:50e50ee8e08a 488
shundai 1:844acc585d3d 489 else
shundai 1:844acc585d3d 490 {
shundai 2:50e50ee8e08a 491 MotorDrive(OMEGA_FEEDBACK, motor2.compute());
shundai 1:844acc585d3d 492 }
shundai 1:844acc585d3d 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 2:50e50ee8e08a 500 if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
shundai 1:844acc585d3d 501 {
shundai 1:844acc585d3d 502 SR = ABLE;
shundai 2:50e50ee8e08a 503 PHASE = (float)CCW;
shundai 1:844acc585d3d 504 PWM1 = 0.4f;
shundai 1:844acc585d3d 505 PWM2 = 1.0f;
shundai 1:844acc585d3d 506 //MotorDrive(CURRENT_FEEDBACK, CW, 1.2f);
shundai 1:844acc585d3d 507 }
shundai 1:844acc585d3d 508
shundai 1:844acc585d3d 509 else
shundai 1:844acc585d3d 510 {
shundai 2:50e50ee8e08a 511 MotorDrive(OMEGA_FEEDBACK, motor2.compute());
shundai 1:844acc585d3d 512 }
shundai 1:844acc585d3d 513
shundai 1:844acc585d3d 514 //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
shundai 1:844acc585d3d 515 }
shundai 1:844acc585d3d 516
shundai 1:844acc585d3d 517 /*
shundai 1:844acc585d3d 518 SR = ABLE;
shundai 1:844acc585d3d 519 PHASE = (float)direction;
shundai 1:844acc585d3d 520 PWM1 = motor.compute();
shundai 1:844acc585d3d 521 PWM2 = 1.0f;
shundai 1:844acc585d3d 522 */
shundai 1:844acc585d3d 523 break;
shundai 1:844acc585d3d 524
shundai 1:844acc585d3d 525 case OMEGA_FEEDBACK:
shundai 1:844acc585d3d 526
shundai 2:50e50ee8e08a 527 /*
shundai 1:844acc585d3d 528 if(direction != old_direction) //Prevent sudden turn
shundai 1:844acc585d3d 529 {
shundai 1:844acc585d3d 530 motor.setProcessValue(0.0f);
shundai 1:844acc585d3d 531
shundai 1:844acc585d3d 532 if(motor.compute() < 0.05f)
shundai 1:844acc585d3d 533 {
shundai 1:844acc585d3d 534 old_direction = direction;
shundai 1:844acc585d3d 535 }
shundai 1:844acc585d3d 536 }
shundai 2:50e50ee8e08a 537 */
shundai 1:844acc585d3d 538
shundai 2:50e50ee8e08a 539
shundai 2:50e50ee8e08a 540 //pc.printf("OMEGA_FEEDBACK ");
shundai 2:50e50ee8e08a 541
shundai 2:50e50ee8e08a 542 if(target >= 0.0f)
shundai 2:50e50ee8e08a 543 {
shundai 1:844acc585d3d 544 motor.setSetPoint(abs(target));
shundai 1:844acc585d3d 545 motor.setProcessValue(abs(Motor.omega));
shundai 1:844acc585d3d 546 //ここにtargetの正負によって回転方向を帰るプログラムを書く
shundai 1:844acc585d3d 547 SR = ABLE;
shundai 2:50e50ee8e08a 548 PHASE = (float)CW;
shundai 1:844acc585d3d 549 PWM1 = motor.compute();
shundai 1:844acc585d3d 550 PWM2 = 1.0f;
shundai 1:844acc585d3d 551
shundai 2:50e50ee8e08a 552 //pc.printf("%f \r\n", (float)direction);
shundai 2:50e50ee8e08a 553 }
shundai 2:50e50ee8e08a 554
shundai 2:50e50ee8e08a 555 else
shundai 2:50e50ee8e08a 556 {
shundai 2:50e50ee8e08a 557 motor.setSetPoint(abs(target));
shundai 2:50e50ee8e08a 558 motor.setProcessValue(abs(Motor.omega));
shundai 2:50e50ee8e08a 559 //ここにtargetの正負によって回転方向を帰るプログラムを書く
shundai 2:50e50ee8e08a 560 SR = ABLE;
shundai 2:50e50ee8e08a 561 PHASE = (float)CCW;
shundai 2:50e50ee8e08a 562 PWM1 = motor.compute();
shundai 2:50e50ee8e08a 563 PWM2 = 1.0f;
shundai 2:50e50ee8e08a 564 }
shundai 2:50e50ee8e08a 565
shundai 1:844acc585d3d 566 break;
shundai 1:844acc585d3d 567
shundai 1:844acc585d3d 568 case CURRENT_FEEDBACK:
shundai 1:844acc585d3d 569
shundai 1:844acc585d3d 570 pc.printf("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
shundai 1:844acc585d3d 571
shundai 1:844acc585d3d 572 break;
shundai 1:844acc585d3d 573
shundai 1:844acc585d3d 574 default:
shundai 1:844acc585d3d 575
shundai 1:844acc585d3d 576 pc.printf("Function:MotorDriver mode error! \r\n");
shundai 1:844acc585d3d 577
shundai 1:844acc585d3d 578 break;
shundai 1:844acc585d3d 579 }
shundai 1:844acc585d3d 580
shundai 1:844acc585d3d 581
shundai 1:844acc585d3d 582 //motor.setProcessValue(omega);
shundai 1:844acc585d3d 583
shundai 1:844acc585d3d 584 //old_direction = direction;
shundai 1:844acc585d3d 585 old_target = target;
shundai 1:844acc585d3d 586 }
shundai 1:844acc585d3d 587
shundai 1:844acc585d3d 588
shundai 1:844acc585d3d 589 /*
shundai 1:844acc585d3d 590 void CAN2DATA()
shundai 1:844acc585d3d 591
shundai 1:844acc585d3d 592 void DATA2CAN()
shundai 1:844acc585d3d 593 */