Anon Anon / Mbed 2 deprecated Nucleo_pid_pwm

Code

Dependencies:   mbed

actions.h@0:8adde5eaea5a, 2017-11-08 (annotated)

Committer:
Showboo
Date:
Wed Nov 08 20:30:42 2017 +0000
Revision:
0:8adde5eaea5a
Prev

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Showboo 0:8adde5eaea5a 1 #ifndef ACTIONS_H
Showboo 0:8adde5eaea5a 2 #define ACTIONS_H
Showboo 0:8adde5eaea5a 3
Showboo 0:8adde5eaea5a 4 PwmOut mypwm(PWM_OUT);
Showboo 0:8adde5eaea5a 5 DigitalOut myled(LED1);
Showboo 0:8adde5eaea5a 6 //Left Motor
Showboo 0:8adde5eaea5a 7 PwmOut m1F(PC_7);
Showboo 0:8adde5eaea5a 8 PwmOut m1B(PB_10);
Showboo 0:8adde5eaea5a 9 //Right Motor
Showboo 0:8adde5eaea5a 10 PwmOut m2F(PA_7);
Showboo 0:8adde5eaea5a 11 PwmOut m2B(PB_6);
Showboo 0:8adde5eaea5a 12 //Left Encoder
Showboo 0:8adde5eaea5a 13 AnalogIn lFront(PB_3);
Showboo 0:8adde5eaea5a 14 AnalogIn lBack(PA_15);
Showboo 0:8adde5eaea5a 15 //Right Encoder
Showboo 0:8adde5eaea5a 16 AnalogIn rFront(PA_1);
Showboo 0:8adde5eaea5a 17 AnalogIn rBack(PC_14);
Showboo 0:8adde5eaea5a 18
Showboo 0:8adde5eaea5a 19 Timer time;
Showboo 0:8adde5eaea5a 20
Showboo 0:8adde5eaea5a 21 struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
Showboo 0:8adde5eaea5a 22 pid(){
Showboo 0:8adde5eaea5a 23 integral = 0.08f;
Showboo 0:8adde5eaea5a 24 prev = 0.0f;
Showboo 0:8adde5eaea5a 25 kp = 1.0f; //the ks should be negative to counteract error
Showboo 0:8adde5eaea5a 26 ki = 0.0f;
Showboo 0:8adde5eaea5a 27 kd = 0.8f;
Showboo 0:8adde5eaea5a 28 }
Showboo 0:8adde5eaea5a 29 float integral;
Showboo 0:8adde5eaea5a 30 int prev;
Showboo 0:8adde5eaea5a 31 float kp; //the ks should be negative to counteract error
Showboo 0:8adde5eaea5a 32 float ki;
Showboo 0:8adde5eaea5a 33 float kd;
Showboo 0:8adde5eaea5a 34 };
Showboo 0:8adde5eaea5a 35
Showboo 0:8adde5eaea5a 36 void resetPid(struct pid *e) {
Showboo 0:8adde5eaea5a 37 e->integral = 0.0f;
Showboo 0:8adde5eaea5a 38 e->prev = 0.0f;
Showboo 0:8adde5eaea5a 39 }
Showboo 0:8adde5eaea5a 40
Showboo 0:8adde5eaea5a 41 float getFix(struct pid *e, float error) {
Showboo 0:8adde5eaea5a 42 //float d = (error - e->prev)/DT;
Showboo 0:8adde5eaea5a 43 e->integral += error * DT;
Showboo 0:8adde5eaea5a 44 e->prev = error;
Showboo 0:8adde5eaea5a 45 }
Showboo 0:8adde5eaea5a 46
Showboo 0:8adde5eaea5a 47
Showboo 0:8adde5eaea5a 48 void turnleft_loop(float lencoder, float rencoder){ //4096 per turn of wheel
Showboo 0:8adde5eaea5a 49 while(lencoder < 12861.0f){
Showboo 0:8adde5eaea5a 50 float error = lencoder - 12861.0f;
Showboo 0:8adde5eaea5a 51 float delta_motor = getFix(lturn, error);
Showboo 0:8adde5eaea5a 52 m1F.period(0.01f);
Showboo 0:8adde5eaea5a 53 m1F = m1basespeed + delta_motor;
Showboo 0:8adde5eaea5a 54 }
Showboo 0:8adde5eaea5a 55 while(rencoder < 12861f){
Showboo 0:8adde5eaea5a 56 float error = rencoder - 12861f;
Showboo 0:8adde5eaea5a 57 float delta_motor = getFix(rturn, error);
Showboo 0:8adde5eaea5a 58 m2B.period(0.01f);
Showboo 0:8adde5eaea5a 59 m2B = m2basespeed + delta_motor;
Showboo 0:8adde5eaea5a 60 }
Showboo 0:8adde5eaea5a 61 }
Showboo 0:8adde5eaea5a 62
Showboo 0:8adde5eaea5a 63
Showboo 0:8adde5eaea5a 64
Showboo 0:8adde5eaea5a 65
Showboo 0:8adde5eaea5a 66
Showboo 0:8adde5eaea5a 67
Showboo 0:8adde5eaea5a 68
Showboo 0:8adde5eaea5a 69
Showboo 0:8adde5eaea5a 70
Showboo 0:8adde5eaea5a 71
Showboo 0:8adde5eaea5a 72
Showboo 0:8adde5eaea5a 73
Showboo 0:8adde5eaea5a 74
Showboo 0:8adde5eaea5a 75 #endif