main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "PID.h"
00004 
00005 #define PWM_PIN p22
00006 #define MIN_1_PIN p19
00007 #define MIN_2_PIN p20
00008 #define ENC_A_PIN p16
00009 #define ENC_B_PIN p17
00010 #define CURRENT_PIN p18
00011 
00012 #define pi 3.141592654
00013 
00014 #define PWM_PERIOD 0.00005
00015 #define PID_CUR_PERIOD 0.001
00016 #define PID_POS_PERIOD 0.005
00017 
00018 #define PID_POS_P_GAIN  0.5
00019 #define PID_POS_D_GAIN  10.0
00020 #define PID_POS_I_GAIN  0.1
00021 
00022 #define PID_CUR_P_GAIN  0.5
00023 #define PID_CUR_D_GAIN  0.0
00024 //#define PID_CUR_I_GAIN  0.001
00025 #define PID_CUR_I_GAIN  0.01
00026 
00027 Serial pc(USBTX, USBRX);
00028 
00029 
00030 Motor motor(
00031     PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD, 
00032     CURRENT_PIN,
00033     ENC_A_PIN, ENC_B_PIN, 
00034     PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN
00035 );
00036 
00037 HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
00038 //CurrentSense current_sense(CURRENT_PIN);
00039 Encoder encoder(ENC_A_PIN, ENC_B_PIN);
00040 
00041 PIDController position_controller(
00042     PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN
00043 );
00044 
00045 Ticker current_ticker;
00046 
00047 void update_current(void) {
00048     motor.update();
00049 }
00050 
00051 Ticker position_ticker;
00052 
00053 void update_position(void) {
00054     motor.set_command(position_controller.command_position(motor.get_position()));
00055 }
00056 
00057 DigitalOut led1(LED1);
00058 DigitalOut led2(LED2);
00059 DigitalOut led3(LED3);
00060 
00061 int main() {
00062     wait(5);
00063     pc.printf("Built " __DATE__ " " __TIME__ "\r\n");
00064     
00065     led1 = 1;
00066     led2 = 1;
00067     led3 = 1;
00068     
00069     wait(5);
00070     
00071     led2 = 0;
00072     pc.printf("Initializing\n\r");
00073     
00074   //  current_ticker.attach(&update_current, PID_CUR_PERIOD);
00075    // position_ticker.attach(&update_position, PID_POS_PERIOD);
00076     
00077     int count = 0;
00078     float command = 0.0;
00079   //  wait(5);
00080     led3 = 0;
00081     pc.printf("Starting\n\r");
00082     
00083     hbridge.initialize();
00084     
00085     while(1){
00086       //command = 0.05 * abs((50-count)/10) - 0.1;
00087        //motor.set_command(command);
00088        command = sin(count*pi/50);
00089         if(command>0)
00090         {
00091             hbridge.forward();
00092         }
00093         else if(command<0)
00094         {
00095              hbridge.back();
00096         }
00097         
00098         led1 = (count % 2);
00099 
00100         
00101         pc.printf("Position:%d\n\r",(int) encoder.get_position());
00102         //printf("PC:%F\n\r", position_controller.command);
00103         //pc.printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", 
00104          //   motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
00105        //);
00106         
00107         
00108         count++;
00109         wait(0.01);
00110     }
00111 }