shundai miyawaki / Mbed 2 deprecated MotorDriver_ver4-1

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

Dependencies:   mbed QEI PID DBG

MotorDriver_ver4-1.cpp@6:cc51f9e7ef18, 2019-08-26 (annotated)

Committer:
shundai
Date:
Mon Aug 26 14:04:08 2019 +0000
Revision:
6:cc51f9e7ef18
Parent:
3:141dc1666df2 
train test ;

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 6:cc51f9e7ef18 50 #define MOTOR_FREUENCY 20000 //[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 6:cc51f9e7ef18 144 float buf = 0.0f;
shundai 1:844acc585d3d 145
shundai 1:844acc585d3d 146
shundai 1:844acc585d3d 147 int main()
shundai 1:844acc585d3d 148 {
shundai 1:844acc585d3d 149 // MPU setup
shundai 1:844acc585d3d 150 pc.printf("MotorDriver_ver4.1 start! \r\n");
shundai 1:844acc585d3d 151 pc.printf("/** MPU INFO **/ \r\n");
shundai 1:844acc585d3d 152 pc.printf("Target PlatForm: STM32F303K8T6 \r\n");
shundai 1:844acc585d3d 153 pc.printf("System Frequency: %d Hz \r\n", SystemCoreClock);
shundai 1:844acc585d3d 154 pc.printf("Program File: %s \r\n", __FILE__);
shundai 1:844acc585d3d 155 pc.printf("Compile Date: %s \r\n", __DATE__);
shundai 1:844acc585d3d 156 pc.printf("Compile Time: %s \r\n", __TIME__);
shundai 1:844acc585d3d 157
shundai 1:844acc585d3d 158 //Unique ID starting addresses for most of STM32 microcontrollers.
shundai 1:844acc585d3d 159 unsigned long *id = (unsigned long *)0x1FFFF7AC;
shundai 1:844acc585d3d 160
shundai 1:844acc585d3d 161 pc.printf("%d, %d, %d \r\n", id[0], id[1], id[2]);
shundai 1:844acc585d3d 162
shundai 1:844acc585d3d 163
shundai 1:844acc585d3d 164 myled = 1;
shundai 1:844acc585d3d 165 wait(1.0);
shundai 1:844acc585d3d 166 myled = 0;
shundai 1:844acc585d3d 167
shundai 1:844acc585d3d 168 //load Flash Data
shundai 1:844acc585d3d 169 int deviceID = 10;
shundai 1:844acc585d3d 170
shundai 1:844acc585d3d 171 // CAN setup
shundai 1:844acc585d3d 172 can.frequency(1000000); //ROS default bps:100kbps -> 500kbps
shundai 1:844acc585d3d 173 //can.filter(deviceID, 0b11111111, CANAny, 0);
shundai 1:844acc585d3d 174 can.reset(); //delete receive buffer
shundai 1:844acc585d3d 175 //can.monitor();
shundai 1:844acc585d3d 176 //can.mode();
shundai 1:844acc585d3d 177
shundai 1:844acc585d3d 178 // motor signal setup
shundai 1:844acc585d3d 179 PWM1.period(1.0f/MOTOR_FREUENCY);
shundai 1:844acc585d3d 180 PWM2.period(1.0f/MOTOR_FREUENCY);
shundai 1:844acc585d3d 181 PHASE.period(1.0f/MOTOR_FREUENCY);
shundai 1:844acc585d3d 182
shundai 1:844acc585d3d 183 PWM1 = 0.0f;
shundai 1:844acc585d3d 184 PWM2 = 0.0f;
shundai 6:cc51f9e7ef18 185 PHASE = 1.0f;
shundai 1:844acc585d3d 186 SR = 0;
shundai 1:844acc585d3d 187
shundai 1:844acc585d3d 188 // PID omega setup
shundai 1:844acc585d3d 189 motor.setInputLimits(0.0f, 3.68f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
shundai 1:844acc585d3d 190 motor.setOutputLimits(0.0f, 1.0f);
shundai 1:844acc585d3d 191 motor.setBias(0.0);
shundai 1:844acc585d3d 192 motor.setMode(AUTO_MODE);
shundai 1:844acc585d3d 193 //motor.setSetPoint(1.0f); //PID target value
shundai 1:844acc585d3d 194
shundai 1:844acc585d3d 195 // PID position setup
shundai 1:844acc585d3d 196 motor2.setInputLimits(-360.0f, 360.0f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
shundai 1:844acc585d3d 197 motor2.setOutputLimits(-3.68f, 3.68f);
shundai 1:844acc585d3d 198 motor2.setBias(0.0);
shundai 1:844acc585d3d 199 motor2.setMode(AUTO_MODE);
shundai 1:844acc585d3d 200 //motor.setSetPoint(1.0f); //PID target value
shundai 1:844acc585d3d 201
shundai 1:844acc585d3d 202 // current sensor setup
shundai 1:844acc585d3d 203 for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++)
shundai 1:844acc585d3d 204 {
shundai 1:844acc585d3d 205 current_bias += Current.read();
shundai 1:844acc585d3d 206 }
shundai 1:844acc585d3d 207
shundai 1:844acc585d3d 208 current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT;
shundai 1:844acc585d3d 209 pc.printf("current_bias_average: %f \r\n", current_bias);
shundai 1:844acc585d3d 210
shundai 1:844acc585d3d 211
shundai 1:844acc585d3d 212
shundai 1:844acc585d3d 213 // interrupt start function
shundai 1:844acc585d3d 214 userButton.mode(PullUp);
shundai 1:844acc585d3d 215 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 216 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 217 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 218 ENCODER_timer.start();
shundai 1:844acc585d3d 219 Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
shundai 1:844acc585d3d 220 //NVIC_SetPriority(TIMER3_IRQn, 1);
shundai 1:844acc585d3d 221
shundai 2:50e50ee8e08a 222 Motor.target = 250.0f; //[deg]
shundai 1:844acc585d3d 223 //Motor.target = 1.0f; //[rps]
shundai 1:844acc585d3d 224
shundai 1:844acc585d3d 225 while(1)
shundai 1:844acc585d3d 226 {
shundai 6:cc51f9e7ef18 227 /*
shundai 6:cc51f9e7ef18 228 pc.printf("buf: %f \r\n", buf);
shundai 6:cc51f9e7ef18 229
shundai 6:cc51f9e7ef18 230 if(buf > 0.0f)
shundai 6:cc51f9e7ef18 231 {
shundai 6:cc51f9e7ef18 232 SR = ABLE;
shundai 6:cc51f9e7ef18 233 PHASE = (float)CW;
shundai 6:cc51f9e7ef18 234 PWM1 = buf;
shundai 6:cc51f9e7ef18 235 PWM2 = 1.0f;
shundai 6:cc51f9e7ef18 236 }
shundai 6:cc51f9e7ef18 237
shundai 6:cc51f9e7ef18 238 else if(buf <= 0.0f)
shundai 6:cc51f9e7ef18 239 {
shundai 6:cc51f9e7ef18 240 SR = ABLE;
shundai 6:cc51f9e7ef18 241 PHASE = (float)CCW;
shundai 6:cc51f9e7ef18 242 PWM1 = -1*buf;
shundai 6:cc51f9e7ef18 243 PWM2 = 1.0f;
shundai 6:cc51f9e7ef18 244 }
shundai 6:cc51f9e7ef18 245 */
shundai 6:cc51f9e7ef18 246
shundai 2:50e50ee8e08a 247 //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
shundai 1:844acc585d3d 248
shundai 2:50e50ee8e08a 249 //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 6:cc51f9e7ef18 250 //pc.printf("theta:%6.3f omega:%6.3f\r\n", Motor.theta, Motor.omega);
shundai 1:844acc585d3d 251
shundai 6:cc51f9e7ef18 252 //MotorDrive(THETA_FEEDBACK, Motor.target);
shundai 6:cc51f9e7ef18 253 MotorDrive(OMEGA_FEEDBACK, Motor.target);
shundai 2:50e50ee8e08a 254
shundai 1:844acc585d3d 255 //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());
shundai 1:844acc585d3d 256 //CurrentFunction();
shundai 1:844acc585d3d 257 }
shundai 1:844acc585d3d 258 }
shundai 1:844acc585d3d 259
shundai 1:844acc585d3d 260 void SerialFunction()
shundai 1:844acc585d3d 261 {
shundai 1:844acc585d3d 262 if(pc.readable() == 1)
shundai 1:844acc585d3d 263 {
shundai 1:844acc585d3d 264 char data = pc.getc();
shundai 1:844acc585d3d 265 pc.printf("get data:%c \r\n", data);
shundai 1:844acc585d3d 266
shundai 1:844acc585d3d 267
shundai 1:844acc585d3d 268 switch (data)
shundai 1:844acc585d3d 269 {
shundai 1:844acc585d3d 270 case 'q':
shundai 1:844acc585d3d 271
shundai 1:844acc585d3d 272 //motor.setSetPoint(1.5f); //PID target value
shundai 1:844acc585d3d 273
shundai 1:844acc585d3d 274 Motor.target += 0.5f;
shundai 1:844acc585d3d 275 pc.printf("get 'q' \r\n");
shundai 1:844acc585d3d 276
shundai 1:844acc585d3d 277 break;
shundai 1:844acc585d3d 278
shundai 1:844acc585d3d 279 case 'a':
shundai 1:844acc585d3d 280
shundai 1:844acc585d3d 281 //motor.setSetPoint(0.5f); //PID target value
shundai 1:844acc585d3d 282
shundai 1:844acc585d3d 283 Motor.target -= 0.5f;
shundai 1:844acc585d3d 284 pc.printf("get 'a' \r\n");
shundai 1:844acc585d3d 285
shundai 1:844acc585d3d 286 break;
shundai 2:50e50ee8e08a 287
shundai 2:50e50ee8e08a 288 case 'p':
shundai 2:50e50ee8e08a 289
shundai 2:50e50ee8e08a 290 //motor.setSetPoint(1.5f); //PID target value
shundai 6:cc51f9e7ef18 291 buf += 0.15f;
shundai 2:50e50ee8e08a 292 Motor.target += 30.0f;
shundai 2:50e50ee8e08a 293 pc.printf("get 'p' \r\n");
shundai 2:50e50ee8e08a 294
shundai 2:50e50ee8e08a 295 break;
shundai 2:50e50ee8e08a 296
shundai 2:50e50ee8e08a 297 case 'l':
shundai 2:50e50ee8e08a 298
shundai 2:50e50ee8e08a 299 //motor.setSetPoint(0.5f); //PID target value
shundai 6:cc51f9e7ef18 300 buf -= 0.15f;
shundai 2:50e50ee8e08a 301 Motor.target -= 30.0f;
shundai 2:50e50ee8e08a 302 pc.printf("get 'l' \r\n");
shundai 2:50e50ee8e08a 303
shundai 2:50e50ee8e08a 304 break;
shundai 2:50e50ee8e08a 305
shundai 1:844acc585d3d 306
shundai 1:844acc585d3d 307 case 'r':
shundai 1:844acc585d3d 308
shundai 1:844acc585d3d 309
shundai 1:844acc585d3d 310
shundai 1:844acc585d3d 311 pc.printf("\r\nSystem Reset! \r\n");
shundai 1:844acc585d3d 312 NVIC_SystemReset();
shundai 1:844acc585d3d 313
shundai 1:844acc585d3d 314 break;
shundai 1:844acc585d3d 315
shundai 1:844acc585d3d 316 default:
shundai 1:844acc585d3d 317
shundai 1:844acc585d3d 318 pc.printf("default \r\n");
shundai 1:844acc585d3d 319
shundai 1:844acc585d3d 320 break;
shundai 1:844acc585d3d 321 }
shundai 1:844acc585d3d 322 }
shundai 1:844acc585d3d 323 }
shundai 1:844acc585d3d 324
shundai 1:844acc585d3d 325 void ButtonFunction()
shundai 1:844acc585d3d 326 {
shundai 1:844acc585d3d 327 motor.setProcessValue(0); //target value = 0
shundai 1:844acc585d3d 328 //int cmd = 0;
shundai 1:844acc585d3d 329
shundai 1:844acc585d3d 330 pc.printf("setup open \r\n");
shundai 1:844acc585d3d 331
shundai 1:844acc585d3d 332 pc.printf("1:change the CAN device ID \r\n");
shundai 1:844acc585d3d 333 pc.printf("2:change the CAN device ID \r\n");
shundai 1:844acc585d3d 334
shundai 1:844acc585d3d 335 //CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
shundai 1:844acc585d3d 336
shundai 1:844acc585d3d 337 //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
shundai 1:844acc585d3d 338
shundai 1:844acc585d3d 339 while(1)
shundai 1:844acc585d3d 340 {
shundai 1:844acc585d3d 341 if(!pc.readable())
shundai 1:844acc585d3d 342 {
shundai 1:844acc585d3d 343 // pc.scanf("%d", cmd);
shundai 1:844acc585d3d 344 }
shundai 1:844acc585d3d 345
shundai 1:844acc585d3d 346 else
shundai 1:844acc585d3d 347 {
shundai 1:844acc585d3d 348 pc.printf("old device ID:\r\n");
shundai 1:844acc585d3d 349 pc.printf("new device ID:\r\n");
shundai 1:844acc585d3d 350
shundai 1:844acc585d3d 351 while(1)
shundai 1:844acc585d3d 352 {
shundai 1:844acc585d3d 353
shundai 1:844acc585d3d 354 }
shundai 1:844acc585d3d 355 }
shundai 1:844acc585d3d 356 }
shundai 1:844acc585d3d 357 }
shundai 1:844acc585d3d 358
shundai 1:844acc585d3d 359 void CANread()
shundai 1:844acc585d3d 360 {
shundai 1:844acc585d3d 361 __disable_irq(); //All Interrupt disabled
shundai 1:844acc585d3d 362 Encoder.detach();
shundai 1:844acc585d3d 363
shundai 1:844acc585d3d 364
shundai 1:844acc585d3d 365 if(can.read(msg) != 0)
shundai 1:844acc585d3d 366 {
shundai 1:844acc585d3d 367 pc.printf("CAN recieve \r\n");
shundai 1:844acc585d3d 368 myled = !myled;
shundai 1:844acc585d3d 369
shundai 1:844acc585d3d 370 for(int i = 0 ; i < 8 ; i++)
shundai 1:844acc585d3d 371 {
shundai 1:844acc585d3d 372 MDCAN.recvDATA[i] = msg.data[i];
shundai 1:844acc585d3d 373 pc.printf("[%c] ", MDCAN.recvDATA[i]);
shundai 1:844acc585d3d 374 }
shundai 1:844acc585d3d 375
shundai 1:844acc585d3d 376 pc.printf("\r\n");
shundai 1:844acc585d3d 377
shundai 1:844acc585d3d 378 switch(MDCAN.recvDATA[1]) //check PID
shundai 1:844acc585d3d 379 {
shundai 1:844acc585d3d 380 case 0: //MD data send
shundai 1:844acc585d3d 381
shundai 1:844acc585d3d 382 MDCAN.sendDATA[0] = 1; //PID
shundai 1:844acc585d3d 383 MDCAN.sendDATA[1] = 0; //dataA
shundai 1:844acc585d3d 384 MDCAN.sendDATA[2] = 0; //dataB
shundai 1:844acc585d3d 385 MDCAN.sendDATA[3] = 0; //dataC
shundai 1:844acc585d3d 386 MDCAN.sendDATA[4] = 0; //dataD
shundai 1:844acc585d3d 387 MDCAN.sendDATA[5] = 0; //dataE
shundai 1:844acc585d3d 388 MDCAN.sendDATA[6] = 0; //dataF
shundai 1:844acc585d3d 389 MDCAN.sendDATA[7] = 15;//dataG
shundai 1:844acc585d3d 390
shundai 1:844acc585d3d 391 //CANsend(hostID);
shundai 1:844acc585d3d 392
shundai 1:844acc585d3d 393 //pc.printf("reply to host(PID=%d)\r\n",PID);
shundai 1:844acc585d3d 394
shundai 1:844acc585d3d 395 break;
shundai 1:844acc585d3d 396
shundai 1:844acc585d3d 397 case 9: //MD drive
shundai 1:844acc585d3d 398
shundai 1:844acc585d3d 399 ///MD4.Drive();
shundai 1:844acc585d3d 400
shundai 1:844acc585d3d 401 break;
shundai 1:844acc585d3d 402
shundai 1:844acc585d3d 403 default:
shundai 1:844acc585d3d 404
shundai 1:844acc585d3d 405 //CAN.recvDATA[1] = 0b00000000;
shundai 1:844acc585d3d 406
shundai 1:844acc585d3d 407 break;
shundai 1:844acc585d3d 408 }
shundai 1:844acc585d3d 409 }
shundai 1:844acc585d3d 410
shundai 1:844acc585d3d 411 motor.setSetPoint((double)MDCAN.recvDATA[1]); //motor target value velocity
shundai 1:844acc585d3d 412 __enable_irq();
shundai 1:844acc585d3d 413 Encoder.attach(Velocity, 0.01); //Measure rotation speed every 10ms
shundai 1:844acc585d3d 414 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 415 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 416 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 417 }
shundai 1:844acc585d3d 418
shundai 1:844acc585d3d 419 void Velocity()
shundai 1:844acc585d3d 420 {
shundai 1:844acc585d3d 421 static int pulse; //old pulse
shundai 1:844acc585d3d 422
shundai 1:844acc585d3d 423 Motor.omega = ((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read();
shundai 1:844acc585d3d 424 pulse = encoder.getPulses(); //pulse update
shundai 1:844acc585d3d 425
shundai 1:844acc585d3d 426 Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f;
shundai 1:844acc585d3d 427
shundai 1:844acc585d3d 428 ENCODER_timer.reset(); //timer reset:t=0s
shundai 1:844acc585d3d 429
shundai 1:844acc585d3d 430 //pc.printf("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
shundai 1:844acc585d3d 431 }
shundai 1:844acc585d3d 432
shundai 1:844acc585d3d 433 void CANsend(int id)
shundai 1:844acc585d3d 434 {
shundai 1:844acc585d3d 435 __disable_irq();
shundai 1:844acc585d3d 436 Encoder.detach();
shundai 1:844acc585d3d 437
shundai 1:844acc585d3d 438 if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended)))
shundai 1:844acc585d3d 439 {
shundai 1:844acc585d3d 440 pc.printf("CAN send error! \r\n");
shundai 1:844acc585d3d 441 }
shundai 1:844acc585d3d 442
shundai 1:844acc585d3d 443 __enable_irq();
shundai 1:844acc585d3d 444 Encoder.attach(Velocity, 0.03); //Measure rotation speed every 30ms
shundai 1:844acc585d3d 445 pc.attach(SerialFunction, Serial::RxIrq);
shundai 1:844acc585d3d 446 userButton.fall(ButtonFunction);
shundai 1:844acc585d3d 447 can.attach(CANread, CAN::RxIrq);
shundai 1:844acc585d3d 448 }
shundai 1:844acc585d3d 449
shundai 1:844acc585d3d 450 /*
shundai 1:844acc585d3d 451 void CANtimeout()
shundai 1:844acc585d3d 452 {
shundai 1:844acc585d3d 453 if(MDCAN.recvDATA[2] == 0)
shundai 1:844acc585d3d 454 {
shundai 1:844acc585d3d 455 pc.printf("CAN CONNECTION TIMEOUT \r\n");
shundai 1:844acc585d3d 456 //MotorDrive();
shundai 1:844acc585d3d 457 }
shundai 1:844acc585d3d 458 }
shundai 1:844acc585d3d 459
shundai 1:844acc585d3d 460 void CurrentFunction()
shundai 1:844acc585d3d 461 {
shundai 1:844acc585d3d 462 static int noneDrive_count;
shundai 1:844acc585d3d 463
shundai 1:844acc585d3d 464
shundai 1:844acc585d3d 465 pc.printf("current(raw) : %f \r\n",Current.read());
shundai 1:844acc585d3d 466
shundai 1:844acc585d3d 467 Motor.current = (Current.read() - current_bias);
shundai 1:844acc585d3d 468
shundai 1:844acc585d3d 469 if(noneDrive_count >= 2)
shundai 1:844acc585d3d 470 {
shundai 1:844acc585d3d 471 CANsend(); //Power capacity lack. check power supply output current
shundai 1:844acc585d3d 472 }
shundai 1:844acc585d3d 473
shundai 1:844acc585d3d 474 //if(MAX_LIMIT_CURRENT)
shundai 1:844acc585d3d 475
shundai 1:844acc585d3d 476 }
shundai 1:844acc585d3d 477 */
shundai 1:844acc585d3d 478
shundai 2:50e50ee8e08a 479 void MotorDrive(int mode, double target)
shundai 1:844acc585d3d 480 {
shundai 1:844acc585d3d 481 //theta feebback(mode 0)
shundai 1:844acc585d3d 482 //omega feedback(mode 1)
shundai 1:844acc585d3d 483 //current feedback(mode 2)
shundai 1:844acc585d3d 484
shundai 1:844acc585d3d 485 //static int old_direction;
shundai 1:844acc585d3d 486 static double old_target;
shundai 1:844acc585d3d 487
shundai 1:844acc585d3d 488 switch (mode)
shundai 1:844acc585d3d 489 {
shundai 1:844acc585d3d 490 case THETA_FEEDBACK:
shundai 1:844acc585d3d 491
shundai 2:50e50ee8e08a 492 //pc.printf("THETA_FEEDBACK!! \r\n");
shundai 1:844acc585d3d 493
shundai 1:844acc585d3d 494 motor2.setSetPoint(target);
shundai 1:844acc585d3d 495 motor2.setProcessValue(Motor.theta);
shundai 1:844acc585d3d 496
shundai 2:50e50ee8e08a 497 if(target >= 0)
shundai 1:844acc585d3d 498 {
shundai 3:141dc1666df2 499 if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止中
shundai 1:844acc585d3d 500 {
shundai 1:844acc585d3d 501 SR = ABLE;
shundai 2:50e50ee8e08a 502 PHASE = (float)CW;
shundai 1:844acc585d3d 503 }
shundai 2:50e50ee8e08a 504
shundai 3:141dc1666df2 505 if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
shundai 1:844acc585d3d 506 {
shundai 2:50e50ee8e08a 507 MotorDrive(OMEGA_FEEDBACK, motor2.compute());
shundai 1:844acc585d3d 508 }
shundai 1:844acc585d3d 509
shundai 3:141dc1666df2 510 else //誤差の範囲内ならモータのPWMを停止させる
shundai 3:141dc1666df2 511 {
shundai 3:141dc1666df2 512 MotorDrive(OMEGA_FEEDBACK, 0.0);
shundai 3:141dc1666df2 513 }
shundai 3:141dc1666df2 514
shundai 1:844acc585d3d 515 //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
shundai 1:844acc585d3d 516 }
shundai 1:844acc585d3d 517
shundai 1:844acc585d3d 518 else
shundai 1:844acc585d3d 519 {
shundai 1:844acc585d3d 520
shundai 3:141dc1666df2 521 if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止時
shundai 1:844acc585d3d 522 {
shundai 1:844acc585d3d 523 SR = ABLE;
shundai 2:50e50ee8e08a 524 PHASE = (float)CCW;
shundai 3:141dc1666df2 525 PWM1 = 0.3f;
shundai 1:844acc585d3d 526 PWM2 = 1.0f;
shundai 1:844acc585d3d 527 }
shundai 1:844acc585d3d 528
shundai 3:141dc1666df2 529 if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
shundai 1:844acc585d3d 530 {
shundai 2:50e50ee8e08a 531 MotorDrive(OMEGA_FEEDBACK, motor2.compute());
shundai 1:844acc585d3d 532 }
shundai 1:844acc585d3d 533
shundai 3:141dc1666df2 534 else //誤差の範囲内ならモータのPWMを停止させる
shundai 3:141dc1666df2 535 {
shundai 3:141dc1666df2 536 MotorDrive(OMEGA_FEEDBACK, 0.0);
shundai 3:141dc1666df2 537 PWM1 = 0.0f;
shundai 3:141dc1666df2 538 PWM2 = 1.0f;
shundai 3:141dc1666df2 539 }
shundai 3:141dc1666df2 540
shundai 1:844acc585d3d 541 //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
shundai 1:844acc585d3d 542 }
shundai 1:844acc585d3d 543
shundai 1:844acc585d3d 544 /*
shundai 1:844acc585d3d 545 SR = ABLE;
shundai 1:844acc585d3d 546 PHASE = (float)direction;
shundai 1:844acc585d3d 547 PWM1 = motor.compute();
shundai 1:844acc585d3d 548 PWM2 = 1.0f;
shundai 1:844acc585d3d 549 */
shundai 1:844acc585d3d 550 break;
shundai 1:844acc585d3d 551
shundai 1:844acc585d3d 552 case OMEGA_FEEDBACK:
shundai 1:844acc585d3d 553
shundai 2:50e50ee8e08a 554 /*
shundai 1:844acc585d3d 555 if(direction != old_direction) //Prevent sudden turn
shundai 1:844acc585d3d 556 {
shundai 1:844acc585d3d 557 motor.setProcessValue(0.0f);
shundai 1:844acc585d3d 558
shundai 1:844acc585d3d 559 if(motor.compute() < 0.05f)
shundai 1:844acc585d3d 560 {
shundai 1:844acc585d3d 561 old_direction = direction;
shundai 1:844acc585d3d 562 }
shundai 1:844acc585d3d 563 }
shundai 2:50e50ee8e08a 564 */
shundai 1:844acc585d3d 565
shundai 2:50e50ee8e08a 566
shundai 2:50e50ee8e08a 567 //pc.printf("OMEGA_FEEDBACK ");
shundai 2:50e50ee8e08a 568
shundai 2:50e50ee8e08a 569 if(target >= 0.0f)
shundai 2:50e50ee8e08a 570 {
shundai 1:844acc585d3d 571 motor.setSetPoint(abs(target));
shundai 1:844acc585d3d 572 motor.setProcessValue(abs(Motor.omega));
shundai 1:844acc585d3d 573 //ここにtargetの正負によって回転方向を帰るプログラムを書く
shundai 1:844acc585d3d 574 SR = ABLE;
shundai 2:50e50ee8e08a 575 PHASE = (float)CW;
shundai 1:844acc585d3d 576 PWM1 = motor.compute();
shundai 1:844acc585d3d 577 PWM2 = 1.0f;
shundai 1:844acc585d3d 578
shundai 2:50e50ee8e08a 579 //pc.printf("%f \r\n", (float)direction);
shundai 2:50e50ee8e08a 580 }
shundai 2:50e50ee8e08a 581
shundai 2:50e50ee8e08a 582 else
shundai 2:50e50ee8e08a 583 {
shundai 2:50e50ee8e08a 584 motor.setSetPoint(abs(target));
shundai 2:50e50ee8e08a 585 motor.setProcessValue(abs(Motor.omega));
shundai 2:50e50ee8e08a 586 //ここにtargetの正負によって回転方向を帰るプログラムを書く
shundai 2:50e50ee8e08a 587 SR = ABLE;
shundai 2:50e50ee8e08a 588 PHASE = (float)CCW;
shundai 2:50e50ee8e08a 589 PWM1 = motor.compute();
shundai 2:50e50ee8e08a 590 PWM2 = 1.0f;
shundai 2:50e50ee8e08a 591 }
shundai 2:50e50ee8e08a 592
shundai 1:844acc585d3d 593 break;
shundai 1:844acc585d3d 594
shundai 1:844acc585d3d 595 case CURRENT_FEEDBACK:
shundai 1:844acc585d3d 596
shundai 1:844acc585d3d 597 pc.printf("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
shundai 1:844acc585d3d 598
shundai 1:844acc585d3d 599 break;
shundai 1:844acc585d3d 600
shundai 1:844acc585d3d 601 default:
shundai 1:844acc585d3d 602
shundai 1:844acc585d3d 603 pc.printf("Function:MotorDriver mode error! \r\n");
shundai 1:844acc585d3d 604
shundai 1:844acc585d3d 605 break;
shundai 1:844acc585d3d 606 }
shundai 1:844acc585d3d 607
shundai 1:844acc585d3d 608
shundai 1:844acc585d3d 609 //motor.setProcessValue(omega);
shundai 1:844acc585d3d 610
shundai 1:844acc585d3d 611 //old_direction = direction;
shundai 1:844acc585d3d 612 old_target = target;
shundai 1:844acc585d3d 613 }
shundai 1:844acc585d3d 614
shundai 1:844acc585d3d 615
shundai 1:844acc585d3d 616 /*
shundai 1:844acc585d3d 617 void CAN2DATA()
shundai 1:844acc585d3d 618
shundai 1:844acc585d3d 619 void DATA2CAN()
shundai 1:844acc585d3d 620 */