Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

control.cpp@5:1faeeab28bd1, 2018-06-05 (annotated)

Committer:
mr_wang
Date:
Tue Jun 05 09:34:33 2018 +0000
Revision:
5:1faeeab28bd1
Parent:
4:99891561a38b 
NYP!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mr_wang 4:99891561a38b 1 #include "mbed.h"
mr_wang 4:99891561a38b 2 #include "control.h"
mr_wang 4:99891561a38b 3 #include "behaviour.h"
mr_wang 4:99891561a38b 4
mr_wang 4:99891561a38b 5 void CONTROL_OnOff(CONTROLLER* control)
mr_wang 4:99891561a38b 6 {
mr_wang 4:99891561a38b 7 if(control->presentvalue > control->setpoint)
mr_wang 4:99891561a38b 8 control->output = control->upperlimit;//upperlimit is negative value
mr_wang 4:99891561a38b 9 else
mr_wang 4:99891561a38b 10 if(control->presentvalue < control->setpoint)
mr_wang 4:99891561a38b 11 control->output = control->lowerlimit;
mr_wang 4:99891561a38b 12 else
mr_wang 4:99891561a38b 13 control->output = 0.0;
mr_wang 4:99891561a38b 14 }
mr_wang 4:99891561a38b 15
mr_wang 4:99891561a38b 16 void CONTROL_P(CONTROLLER* control)
mr_wang 4:99891561a38b 17 {
mr_wang 4:99891561a38b 18 control->error = control->setpoint - control->presentvalue;
mr_wang 4:99891561a38b 19 control->error_p = control->error;
mr_wang 4:99891561a38b 20 control->output = control->gain_p * control->error_p;
mr_wang 4:99891561a38b 21
mr_wang 4:99891561a38b 22 if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
mr_wang 4:99891561a38b 23 {
mr_wang 4:99891561a38b 24 if(control->output < control->lowerlimit)
mr_wang 4:99891561a38b 25 control->output = control->lowerlimit;
mr_wang 4:99891561a38b 26 if(control->output > control->upperlimit)
mr_wang 4:99891561a38b 27 control->output = control->upperlimit;
mr_wang 4:99891561a38b 28 }
mr_wang 4:99891561a38b 29 else
mr_wang 4:99891561a38b 30 control->output = 0.0;
mr_wang 4:99891561a38b 31 }
mr_wang 4:99891561a38b 32
mr_wang 4:99891561a38b 33 void CONTROL_PI(CONTROLLER* control)
mr_wang 4:99891561a38b 34 {
mr_wang 4:99891561a38b 35 double error_old;
mr_wang 4:99891561a38b 36
mr_wang 4:99891561a38b 37 error_old = control->error;
mr_wang 4:99891561a38b 38 control->error = control->setpoint - control->presentvalue;
mr_wang 4:99891561a38b 39
mr_wang 4:99891561a38b 40 control->error_p = control->error;
mr_wang 4:99891561a38b 41 control->error_i += error_old;
mr_wang 4:99891561a38b 42 control->output = (control->gain_p * control->error_p)+(control->gain_i * control->error_i);
mr_wang 4:99891561a38b 43
mr_wang 4:99891561a38b 44 if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
mr_wang 4:99891561a38b 45 {
mr_wang 4:99891561a38b 46 if(control->output < control->lowerlimit)
mr_wang 4:99891561a38b 47 control->output = control->lowerlimit;
mr_wang 4:99891561a38b 48 if(control->output > control->upperlimit)
mr_wang 4:99891561a38b 49 control->output = control->upperlimit;
mr_wang 4:99891561a38b 50 }
mr_wang 4:99891561a38b 51 else
mr_wang 4:99891561a38b 52 control->output = 0.0;
mr_wang 4:99891561a38b 53 }
mr_wang 4:99891561a38b 54
mr_wang 4:99891561a38b 55 void CONTROL_PD(CONTROLLER* control)
mr_wang 4:99891561a38b 56 {
mr_wang 4:99891561a38b 57 double error_old;
mr_wang 4:99891561a38b 58
mr_wang 4:99891561a38b 59 error_old = control->error;
mr_wang 4:99891561a38b 60 control->error = control->setpoint - control->presentvalue;
mr_wang 4:99891561a38b 61 control->error_p = control->error;
mr_wang 4:99891561a38b 62 control->error_d = control->error - error_old;
mr_wang 4:99891561a38b 63 control->output = (control->gain_p * control->error_p)+(control->gain_d * control->error_d);
mr_wang 4:99891561a38b 64
mr_wang 4:99891561a38b 65 if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
mr_wang 4:99891561a38b 66 {
mr_wang 4:99891561a38b 67 if(control->output < control->lowerlimit)
mr_wang 4:99891561a38b 68 control->output = control->lowerlimit;
mr_wang 4:99891561a38b 69 if(control->output > control->upperlimit)
mr_wang 4:99891561a38b 70 control->output = control->upperlimit;
mr_wang 4:99891561a38b 71 }
mr_wang 4:99891561a38b 72 else
mr_wang 4:99891561a38b 73 control->output = 0.0;
mr_wang 4:99891561a38b 74 }
mr_wang 4:99891561a38b 75
mr_wang 4:99891561a38b 76 void CONTROL_PID(CONTROLLER* control)
mr_wang 4:99891561a38b 77 {
mr_wang 4:99891561a38b 78 double error_old;
mr_wang 4:99891561a38b 79
mr_wang 4:99891561a38b 80 error_old = control->error;
mr_wang 4:99891561a38b 81 control->error = control->setpoint - control->presentvalue;
mr_wang 4:99891561a38b 82
mr_wang 4:99891561a38b 83 control->error_p = control->error;
mr_wang 4:99891561a38b 84 control->error_i += error_old;
mr_wang 4:99891561a38b 85 control->error_d = control->error - error_old;
mr_wang 4:99891561a38b 86 control->output = (control->gain_p * control->error_p)+(control->gain_i * control->error_i)+(control->gain_d * control->error_d);
mr_wang 4:99891561a38b 87
mr_wang 4:99891561a38b 88 if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
mr_wang 4:99891561a38b 89 {
mr_wang 4:99891561a38b 90 if(control->output < control->lowerlimit)
mr_wang 4:99891561a38b 91 control->output = control->lowerlimit;
mr_wang 4:99891561a38b 92 if(control->output > control->upperlimit)
mr_wang 4:99891561a38b 93 control->output = control->upperlimit;
mr_wang 4:99891561a38b 94 }
mr_wang 4:99891561a38b 95 else
mr_wang 4:99891561a38b 96 control->output = 0.0;
mr_wang 4:99891561a38b 97 }
mr_wang 4:99891561a38b 98
mr_wang 4:99891561a38b 99 void CONTROL_InputMode_Set(CONTROLLER* control, unsigned char input, double mode)
mr_wang 4:99891561a38b 100 {
mr_wang 4:99891561a38b 101 control->input = input;
mr_wang 4:99891561a38b 102 control->mode = mode;
mr_wang 4:99891561a38b 103 }
mr_wang 4:99891561a38b 104 void CONTROL_Setpoint_Set(CONTROLLER* control, double sp)
mr_wang 4:99891561a38b 105 {
mr_wang 4:99891561a38b 106 control->setpoint = sp;
mr_wang 4:99891561a38b 107 }
mr_wang 4:99891561a38b 108 void CONTROL_Gain_Set(CONTROLLER* control, double p, double i, double d)
mr_wang 4:99891561a38b 109 {
mr_wang 4:99891561a38b 110 control->gain_p = p;
mr_wang 4:99891561a38b 111 control->gain_i = i;
mr_wang 4:99891561a38b 112 control->gain_d = d;
mr_wang 4:99891561a38b 113 }
mr_wang 4:99891561a38b 114 void CONTROL_Limit_Set(CONTROLLER* control, double upper, double lower)
mr_wang 4:99891561a38b 115 {
mr_wang 4:99891561a38b 116 control->upperlimit = upper;
mr_wang 4:99891561a38b 117 control->lowerlimit = lower;
mr_wang 4:99891561a38b 118 }
mr_wang 4:99891561a38b 119 void CONTROL_Deadband_Set(CONTROLLER* control, double upper, double lower)
mr_wang 4:99891561a38b 120 {
mr_wang 4:99891561a38b 121 control->upperdeadband = upper;
mr_wang 4:99891561a38b 122 control->lowerdeadband = lower;
mr_wang 4:99891561a38b 123 }
mr_wang 4:99891561a38b 124
mr_wang 4:99891561a38b 125 void CONTROL_PresentValue_Set(CONTROLLER* control, double pv)
mr_wang 4:99891561a38b 126 {
mr_wang 4:99891561a38b 127 control->presentvalue = pv;
mr_wang 4:99891561a38b 128 }
mr_wang 4:99891561a38b 129
mr_wang 4:99891561a38b 130 double CONTROL_Output_Get(CONTROLLER* control)
mr_wang 4:99891561a38b 131 {
mr_wang 4:99891561a38b 132 switch(control->mode)
mr_wang 4:99891561a38b 133 {
mr_wang 4:99891561a38b 134 case CONTROL_MODE_NONE:
mr_wang 4:99891561a38b 135 break;
mr_wang 4:99891561a38b 136 case CONTROL_MODE_ONOFF:
mr_wang 4:99891561a38b 137 CONTROL_OnOff(control);
mr_wang 4:99891561a38b 138 break;
mr_wang 4:99891561a38b 139 case CONTROL_MODE_P:
mr_wang 4:99891561a38b 140 CONTROL_P(control);
mr_wang 4:99891561a38b 141 break;
mr_wang 4:99891561a38b 142 case CONTROL_MODE_PI:
mr_wang 4:99891561a38b 143 CONTROL_PI(control);
mr_wang 4:99891561a38b 144 break;
mr_wang 4:99891561a38b 145 case CONTROL_MODE_PD:
mr_wang 4:99891561a38b 146 CONTROL_PD(control);
mr_wang 4:99891561a38b 147 break;
mr_wang 4:99891561a38b 148 case CONTROL_MODE_PID:
mr_wang 4:99891561a38b 149 CONTROL_PID(control);
mr_wang 4:99891561a38b 150 break;
mr_wang 4:99891561a38b 151 }
mr_wang 4:99891561a38b 152 return control->output;
mr_wang 4:99891561a38b 153 }
mr_wang 4:99891561a38b 154
mr_wang 4:99891561a38b 155 void CONTROL_Config(void)
mr_wang 4:99891561a38b 156 {
mr_wang 4:99891561a38b 157 }
mr_wang 4:99891561a38b 158
mr_wang 4:99891561a38b 159 void CONTROL_Task(void)
mr_wang 4:99891561a38b 160 {
mr_wang 4:99891561a38b 161
mr_wang 4:99891561a38b 162 }