shundai miyawaki / Mbed 2 deprecated MotorDriver_ver4-1

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

Dependencies:   mbed QEI PID DBG

MotorDriver_ver4-1.cpp@4:40a764acc4ec, 2019-07-14 (annotated)

Committer:
shundai
Date:
Sun Jul 14 08:38:04 2019 +0000
Revision:
4:40a764acc4ec
Parent:
3:141dc1666df2
Child:
5:1155a15f978c
motor driver 4.1 ok2;

Who changed what in which revision?

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