main.cpp

00001 #include "mbed.h"
00002 #include "EC12E2420801.h"
00003 #include "pid.h"
00004 
00005 #define SLAVE_ID 0x03
00006 
00007 #define I2C_SDA D4
00008 #define I2C_SCL D5
00009 #define MOTOR_PWM D9
00010 #define MOTOR_DIR D10
00011 #define MOTOR_EN  D12
00012 #define SEN_HALL  A0
00013 #define SEN_ENC_A A2
00014 #define SEN_ENC_B A4
00015 
00016 Serial pc(USBTX, USBRX, 115200);
00017 I2CSlave slave(I2C_SDA, I2C_SCL);
00018 PwmOut mot_pwm(MOTOR_PWM);
00019 DigitalOut mot_dir(MOTOR_DIR);
00020 DigitalOut mot_en(MOTOR_EN);
00021 AnalogIn hall(SEN_HALL);
00022 Encoder encoder(SEN_ENC_A, SEN_ENC_B);
00023 
00024 PositionPid _pid;
00025 Ticker tracer;
00026 Ticker debugger;
00027 
00028 void trace();
00029 void setup();
00030 void debug();
00031 
00032 int position = 0;
00033 
00034 namespace para {
00035   const float PID_KP = 0.5f;
00036   const float PID_KI = 0.0f;
00037   const float PID_KD = 0.00001f;
00038   const float PID_DT = 0.002f;
00039 }
00040 
00041 inline int getDIR(float duty){
00042   return ((duty >= 0)? 1 : 0);
00043 }
00044 
00045 void trace() {    
00046     _pid.calculate(position, int(encoder.getData(COUNT)));
00047     mot_dir = getDIR(_pid.duty());
00048     mot_pwm = abs(_pid.duty());
00049 }
00050 void setup() {
00051     slave.address(SLAVE_ID);
00052     debugger.attach(&debug, 0.1);
00053     _pid.setup(para::PID_KP, para::PID_KI, para::PID_KD, para::PID_DT);
00054     mot_en = 0; // enable motor drive
00055 }
00056 void debug() {
00057     pc.printf("%4.3f, ", hall.read());
00058     pc.printf("%d, ", position);
00059     pc.printf("%d\r\n", int(encoder.getData(COUNT)));
00060 }
00061 
00062 int main() {
00063     setup();
00064     
00065     while(1){
00066         if(int(10 * hall.read()) != 0){
00067             mot_dir = 1;
00068             mot_pwm = 0.5;
00069             wait_ms(1.0);
00070         }else{
00071             mot_pwm = 0.0;
00072             encoder.reset();
00073             break;
00074         }
00075     }
00076     
00077     wait(2.0);
00078     pc.printf("start\r\n");
00079 
00080     tracer.attach(&trace, para::PID_DT);
00081    
00082     while(1) {
00083         char buf[10];
00084 
00085         switch (slave.receive()) {
00086             case I2CSlave::NoData:
00087                 break;
00088             case I2CSlave::ReadAddressed:
00089                 break;
00090             case I2CSlave::WriteGeneral:
00091                 break;
00092             case I2CSlave::WriteAddressed:
00093                 slave.read(buf, 10);
00094                 position = int(buf[2]);
00095                 break;
00096         }
00097     }
00098 }