jiexuan fan / ServoController_2

mit

Dependencies:   QEI mbed-src

main.cpp@5:e90c8b57811c, 2018-12-24 (annotated)

Committer:
coldplay
Date:
Mon Dec 24 03:47:49 2018 +0000
Revision:
5:e90c8b57811c
Parent:
4:5ae9f8b3a16f 
mit

Who changed what in which revision?

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