Kazushi Yamanobe
/
R-ciliatable_4
latest version 9/26
Diff: main.cpp
- Revision:
- 0:0d02a451e79d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Oct 10 04:47:14 2021 +0000 @@ -0,0 +1,98 @@ +#include "mbed.h" +#include "EC12E2420801.h" +#include "pid.h" + +#define SLAVE_ID 0x03 + +#define I2C_SDA D4 +#define I2C_SCL D5 +#define MOTOR_PWM D9 +#define MOTOR_DIR D10 +#define MOTOR_EN D12 +#define SEN_HALL A0 +#define SEN_ENC_A A2 +#define SEN_ENC_B A4 + +Serial pc(USBTX, USBRX, 115200); +I2CSlave slave(I2C_SDA, I2C_SCL); +PwmOut mot_pwm(MOTOR_PWM); +DigitalOut mot_dir(MOTOR_DIR); +DigitalOut mot_en(MOTOR_EN); +AnalogIn hall(SEN_HALL); +Encoder encoder(SEN_ENC_A, SEN_ENC_B); + +PositionPid _pid; +Ticker tracer; +Ticker debugger; + +void trace(); +void setup(); +void debug(); + +int position = 0; + +namespace para { + const float PID_KP = 0.5f; + const float PID_KI = 0.0f; + const float PID_KD = 0.00001f; + const float PID_DT = 0.002f; +} + +inline int getDIR(float duty){ + return ((duty >= 0)? 1 : 0); +} + +void trace() { + _pid.calculate(position, int(encoder.getData(COUNT))); + mot_dir = getDIR(_pid.duty()); + mot_pwm = abs(_pid.duty()); +} +void setup() { + slave.address(SLAVE_ID); + debugger.attach(&debug, 0.1); + _pid.setup(para::PID_KP, para::PID_KI, para::PID_KD, para::PID_DT); + mot_en = 0; // enable motor drive +} +void debug() { + pc.printf("%4.3f, ", hall.read()); + pc.printf("%d, ", position); + pc.printf("%d\r\n", int(encoder.getData(COUNT))); +} + +int main() { + setup(); + + while(1){ + if(int(10 * hall.read()) != 0){ + mot_dir = 1; + mot_pwm = 0.5; + wait_ms(1.0); + }else{ + mot_pwm = 0.0; + encoder.reset(); + break; + } + } + + wait(2.0); + pc.printf("start\r\n"); + + tracer.attach(&trace, para::PID_DT); + + while(1) { + char buf[10]; + + switch (slave.receive()) { + case I2CSlave::NoData: + break; + case I2CSlave::ReadAddressed: + break; + case I2CSlave::WriteGeneral: + break; + case I2CSlave::WriteAddressed: + slave.read(buf, 10); + position = int(buf[2]); + break; + } + } +} \ No newline at end of file