jiexuan fan / ServoController_2

mit

Dependencies:   QEI mbed-src

main.cpp@4:5ae9f8b3a16f, 2015-11-24 (annotated)

Committer:
abuchan
Date:
Tue Nov 24 03:56:22 2015 +0000
Revision:
4:5ae9f8b3a16f
Parent:
3:cae0b305d54c
Child:
5:e90c8b57811c
Decoupled position and current control working.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 2:89bb6272869b 1 //Ben Katz, 2013
abuchan 4:5ae9f8b3a16f 2 //Austin Buchan, 2015
abuchan 4:5ae9f8b3a16f 3 #include "mbed.h"
abuchan 4:5ae9f8b3a16f 4 #include "Motor.h"
benkatz 2:89bb6272869b 5 #include "PID.h"
benkatz 2:89bb6272869b 6
abuchan 4:5ae9f8b3a16f 7 #define PWM_PIN PTB13
abuchan 4:5ae9f8b3a16f 8 #define MIN_1_PIN PTA0
abuchan 4:5ae9f8b3a16f 9 #define MIN_2_PIN PTA8
abuchan 4:5ae9f8b3a16f 10 #define ENC_A_PIN PTB6
abuchan 4:5ae9f8b3a16f 11 #define ENC_B_PIN PTB7
abuchan 4:5ae9f8b3a16f 12 #define CURRENT_PIN PTA9
benkatz 2:89bb6272869b 13
abuchan 4:5ae9f8b3a16f 14 #define UART_RX_PIN PTB1
abuchan 4:5ae9f8b3a16f 15 #define UART_TX_PIN PTB2
benkatz 2:89bb6272869b 16
abuchan 4:5ae9f8b3a16f 17 #define PWM_PERIOD 0.001
abuchan 4:5ae9f8b3a16f 18 #define PID_CUR_PERIOD 0.001
abuchan 4:5ae9f8b3a16f 19 #define PID_POS_PERIOD 0.005
abuchan 4:5ae9f8b3a16f 20
abuchan 4:5ae9f8b3a16f 21 #define PID_POS_P_GAIN 0.5
abuchan 4:5ae9f8b3a16f 22 #define PID_POS_D_GAIN 10.0
abuchan 4:5ae9f8b3a16f 23 #define PID_POS_I_GAIN 0.1
aberk 0:bcff39fac858 24
abuchan 4:5ae9f8b3a16f 25 #define PID_CUR_P_GAIN 0.5
abuchan 4:5ae9f8b3a16f 26 #define PID_CUR_D_GAIN 0.0
abuchan 4:5ae9f8b3a16f 27 //#define PID_CUR_I_GAIN 0.001
abuchan 4:5ae9f8b3a16f 28 #define PID_CUR_I_GAIN 0.01
benkatz 3:cae0b305d54c 29
abuchan 4:5ae9f8b3a16f 30 Serial pc(UART_TX_PIN, UART_RX_PIN);
benkatz 2:89bb6272869b 31
abuchan 4:5ae9f8b3a16f 32 Motor motor(
abuchan 4:5ae9f8b3a16f 33 PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD,
abuchan 4:5ae9f8b3a16f 34 CURRENT_PIN,
abuchan 4:5ae9f8b3a16f 35 ENC_A_PIN, ENC_B_PIN,
abuchan 4:5ae9f8b3a16f 36 PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN
abuchan 4:5ae9f8b3a16f 37 );
benkatz 3:cae0b305d54c 38
abuchan 4:5ae9f8b3a16f 39 //HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
abuchan 4:5ae9f8b3a16f 40 //CurrentSense current_sense(CURRENT_PIN);
abuchan 4:5ae9f8b3a16f 41 //Encoder encoder(ENC_A_PIN, ENC_B_PIN);
abuchan 4:5ae9f8b3a16f 42
abuchan 4:5ae9f8b3a16f 43 PIDController position_controller(
abuchan 4:5ae9f8b3a16f 44 PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN
abuchan 4:5ae9f8b3a16f 45 );
abuchan 4:5ae9f8b3a16f 46
abuchan 4:5ae9f8b3a16f 47 Ticker current_ticker;
abuchan 4:5ae9f8b3a16f 48
abuchan 4:5ae9f8b3a16f 49 void update_current(void) {
abuchan 4:5ae9f8b3a16f 50 motor.update();
benkatz 2:89bb6272869b 51 }
benkatz 2:89bb6272869b 52
abuchan 4:5ae9f8b3a16f 53 Ticker position_ticker;
benkatz 3:cae0b305d54c 54
abuchan 4:5ae9f8b3a16f 55 void update_position(void) {
abuchan 4:5ae9f8b3a16f 56 motor.set_command(position_controller.command_position(motor.get_position()));
aberk 0:bcff39fac858 57 }
benkatz 2:89bb6272869b 58
abuchan 4:5ae9f8b3a16f 59 DigitalOut led1(LED1);
abuchan 4:5ae9f8b3a16f 60 DigitalOut led2(LED2);
abuchan 4:5ae9f8b3a16f 61 DigitalOut led3(LED3);
benkatz 2:89bb6272869b 62
abuchan 4:5ae9f8b3a16f 63 int main() {
abuchan 4:5ae9f8b3a16f 64 printf("Built " __DATE__ " " __TIME__ "\r\n");
abuchan 4:5ae9f8b3a16f 65
abuchan 4:5ae9f8b3a16f 66 led1 = 1;
abuchan 4:5ae9f8b3a16f 67 led2 = 1;
abuchan 4:5ae9f8b3a16f 68 led3 = 1;
abuchan 4:5ae9f8b3a16f 69
abuchan 4:5ae9f8b3a16f 70 wait(.5);
abuchan 4:5ae9f8b3a16f 71
abuchan 4:5ae9f8b3a16f 72 led2 = 0;
abuchan 4:5ae9f8b3a16f 73 printf("Initializing\n\r");
abuchan 4:5ae9f8b3a16f 74
abuchan 4:5ae9f8b3a16f 75 current_ticker.attach(&update_current, PID_CUR_PERIOD);
abuchan 4:5ae9f8b3a16f 76 position_ticker.attach(&update_position, PID_POS_PERIOD);
abuchan 4:5ae9f8b3a16f 77
abuchan 4:5ae9f8b3a16f 78 int count = 0;
abuchan 4:5ae9f8b3a16f 79 float command = 0.0;
benkatz 2:89bb6272869b 80
abuchan 4:5ae9f8b3a16f 81 led3 = 0;
abuchan 4:5ae9f8b3a16f 82 printf("Starting\n\r");
benkatz 3:cae0b305d54c 83
abuchan 4:5ae9f8b3a16f 84 while(1){
abuchan 4:5ae9f8b3a16f 85 //command = 0.05 * abs((50-count)/10) - 0.1;
abuchan 4:5ae9f8b3a16f 86 //motor.set_command(command);
abuchan 4:5ae9f8b3a16f 87
abuchan 4:5ae9f8b3a16f 88 command = 1.0 * abs((55-count)/10) - 0.25;
abuchan 4:5ae9f8b3a16f 89 position_controller.set_command(command);
abuchan 4:5ae9f8b3a16f 90
abuchan 4:5ae9f8b3a16f 91 led1 = (count % 2);
abuchan 4:5ae9f8b3a16f 92
abuchan 4:5ae9f8b3a16f 93 //printf("C:%F Cu:%F P:%F\n\r", command, current_sense.get_current(), encoder.get_position());
abuchan 4:5ae9f8b3a16f 94 //printf("PC:%F\n\r", position_controller.command);
abuchan 4:5ae9f8b3a16f 95 printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r",
abuchan 4:5ae9f8b3a16f 96 motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
abuchan 4:5ae9f8b3a16f 97 );
abuchan 4:5ae9f8b3a16f 98
abuchan 4:5ae9f8b3a16f 99
abuchan 4:5ae9f8b3a16f 100 count = (count + 1) % 100;
abuchan 4:5ae9f8b3a16f 101
abuchan 4:5ae9f8b3a16f 102 wait(0.1);
benkatz 3:cae0b305d54c 103 }
abuchan 4:5ae9f8b3a16f 104 }