hige dura / Mbed 2 deprecated yalHackVelFB

Velocity feedback for YAL RChack

Dependencies:   mbed

main.cpp@0:72e0bf0dbe7c, 2014-11-06 (annotated)

Committer:
higedura
Date:
Thu Nov 06 10:13:27 2014 +0000
Revision:
0:72e0bf0dbe7c
Velocity feedback for YAL RChack

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 0:72e0bf0dbe7c 1 #include "mbed.h"
higedura 0:72e0bf0dbe7c 2
higedura 0:72e0bf0dbe7c 3 #define N_CHAN 3 // the numbers of channels
higedura 0:72e0bf0dbe7c 4 #define SIZE_DATA 16 // size of vz data
higedura 0:72e0bf0dbe7c 5
higedura 0:72e0bf0dbe7c 6 Serial pc(USBTX, USBRX);
higedura 0:72e0bf0dbe7c 7
higedura 0:72e0bf0dbe7c 8 DigitalOut led1(LED1);
higedura 0:72e0bf0dbe7c 9 DigitalOut led2(LED2);
higedura 0:72e0bf0dbe7c 10 DigitalOut led3(LED3);
higedura 0:72e0bf0dbe7c 11 DigitalOut led4(LED4);
higedura 0:72e0bf0dbe7c 12
higedura 0:72e0bf0dbe7c 13 //InterruptIn ch1_in(p5); // roll
higedura 0:72e0bf0dbe7c 14 InterruptIn ch2_in(p5); // pitch
higedura 0:72e0bf0dbe7c 15 InterruptIn ch3_in(p6); // throttle
higedura 0:72e0bf0dbe7c 16 //InterruptIn ch4_in(p7); // taw
higedura 0:72e0bf0dbe7c 17 InterruptIn ch5_in(p7); // aux1
higedura 0:72e0bf0dbe7c 18 //InterruptIn ch6_in(p18); // aux2
higedura 0:72e0bf0dbe7c 19
higedura 0:72e0bf0dbe7c 20 DigitalIn button(p12);
higedura 0:72e0bf0dbe7c 21
higedura 0:72e0bf0dbe7c 22 Serial xbee(p13, p14); // tx, rx
higedura 0:72e0bf0dbe7c 23
higedura 0:72e0bf0dbe7c 24 //PwmOut ch1_out(p21); // rall
higedura 0:72e0bf0dbe7c 25 PwmOut ch2_out(p21); // pitch
higedura 0:72e0bf0dbe7c 26 PwmOut ch3_out(p22); // throttle
higedura 0:72e0bf0dbe7c 27 //PwmOut ch4_out(p24); // yaw
higedura 0:72e0bf0dbe7c 28 //PwmOut ch5_out(p25); // aux1
higedura 0:72e0bf0dbe7c 29 //PwmOut ch6_out(p26); // aux2
higedura 0:72e0bf0dbe7c 30
higedura 0:72e0bf0dbe7c 31 ///////////////////// Function prototype ////////////////////
higedura 0:72e0bf0dbe7c 32 //void roll_up(void);
higedura 0:72e0bf0dbe7c 33 void pitch_up(void);
higedura 0:72e0bf0dbe7c 34 void thr_up(void);
higedura 0:72e0bf0dbe7c 35 //void yaw_up(void);
higedura 0:72e0bf0dbe7c 36 void aux1_up(void);
higedura 0:72e0bf0dbe7c 37 //void aux2_up(void);
higedura 0:72e0bf0dbe7c 38
higedura 0:72e0bf0dbe7c 39 //void rall_down(void);
higedura 0:72e0bf0dbe7c 40 void pitch_down(void);
higedura 0:72e0bf0dbe7c 41 void thr_down(void);
higedura 0:72e0bf0dbe7c 42 //void yaw_down(void);
higedura 0:72e0bf0dbe7c 43 void aux1_down(void);
higedura 0:72e0bf0dbe7c 44 //void aux2_down(void);
higedura 0:72e0bf0dbe7c 45
higedura 0:72e0bf0dbe7c 46 ///////////////////// Timers ////////////////////
higedura 0:72e0bf0dbe7c 47 //Timer t_roll;
higedura 0:72e0bf0dbe7c 48 Timer t_pitch;
higedura 0:72e0bf0dbe7c 49 Timer t_thr;
higedura 0:72e0bf0dbe7c 50 //Timer t_yaw;
higedura 0:72e0bf0dbe7c 51 Timer t_aux1;
higedura 0:72e0bf0dbe7c 52 //Timer t_aux2;
higedura 0:72e0bf0dbe7c 53
higedura 0:72e0bf0dbe7c 54 ///////////////////// Global variable ////////////////////
higedura 0:72e0bf0dbe7c 55 int g_pwm_pulse[N_CHAN] = {0}; // pitch thr aux1
higedura 0:72e0bf0dbe7c 56
higedura 0:72e0bf0dbe7c 57 ///////////////////// Local file system ////////////////////
higedura 0:72e0bf0dbe7c 58 LocalFileSystem local("local");
higedura 0:72e0bf0dbe7c 59
higedura 0:72e0bf0dbe7c 60 ///////////////////// Main loop ////////////////////
higedura 0:72e0bf0dbe7c 61 int main() {
higedura 0:72e0bf0dbe7c 62
higedura 0:72e0bf0dbe7c 63 pc.baud(921600);
higedura 0:72e0bf0dbe7c 64 xbee.baud(57600);
higedura 0:72e0bf0dbe7c 65
higedura 0:72e0bf0dbe7c 66 double GAIN_VEL_X = 0.2;
higedura 0:72e0bf0dbe7c 67 //double GAIN_VEL_Z = 0;
higedura 0:72e0bf0dbe7c 68
higedura 0:72e0bf0dbe7c 69 double VEL_X_TARGET = 500;
higedura 0:72e0bf0dbe7c 70
higedura 0:72e0bf0dbe7c 71 double LIMIT_PULTH_WIDTH_CH2_OUT_LOW = 0.0013;
higedura 0:72e0bf0dbe7c 72 double LIMIT_PULTH_WIDTH_CH2_OUT_UP = 0.0017;
higedura 0:72e0bf0dbe7c 73
higedura 0:72e0bf0dbe7c 74 int pos[2] = {0};
higedura 0:72e0bf0dbe7c 75 int vel[2] = {0};
higedura 0:72e0bf0dbe7c 76
higedura 0:72e0bf0dbe7c 77 int PULTH_WIDTH_RC_TRIM_CH2 = 0;
higedura 0:72e0bf0dbe7c 78 int PULTH_WIDTH_RC_TRIM_CH3 = 0;
higedura 0:72e0bf0dbe7c 79
higedura 0:72e0bf0dbe7c 80 double pulse_width_ch2_out = 0.0015;
higedura 0:72e0bf0dbe7c 81 double pulse_width_ch3_out = 0.001;
higedura 0:72e0bf0dbe7c 82
higedura 0:72e0bf0dbe7c 83 ch2_out.period(0.02);
higedura 0:72e0bf0dbe7c 84 ch3_out.period(0.02);
higedura 0:72e0bf0dbe7c 85
higedura 0:72e0bf0dbe7c 86 ch2_out.pulsewidth(0.0015); // pitch
higedura 0:72e0bf0dbe7c 87 ch3_out.pulsewidth(0.0011); // thr
higedura 0:72e0bf0dbe7c 88
higedura 0:72e0bf0dbe7c 89 /////////////////// Interrupt for xbee ////////////////////
higedura 0:72e0bf0dbe7c 90 //xbee.attach(&get_vel);
higedura 0:72e0bf0dbe7c 91
higedura 0:72e0bf0dbe7c 92 //////////////////// Interrupt for RC reciever ////////////////////
higedura 0:72e0bf0dbe7c 93 //ch1_in.rise(&roll_up); ch1_in.fall(&roll_down);
higedura 0:72e0bf0dbe7c 94 ch2_in.rise(&pitch_up); ch2_in.fall(&pitch_down);
higedura 0:72e0bf0dbe7c 95 ch3_in.rise(&thr_up); ch3_in.fall(&thr_down);
higedura 0:72e0bf0dbe7c 96 //ch4_in.rise(&yaw_up); ch4_in.fall(&yaw_down);
higedura 0:72e0bf0dbe7c 97 ch5_in.rise(&aux1_up); ch5_in.fall(&aux1_down);
higedura 0:72e0bf0dbe7c 98 //ch6_in.rise(&aux2_up); ch6_in.fall(&aux2_down);
higedura 0:72e0bf0dbe7c 99
higedura 0:72e0bf0dbe7c 100 // pos初期値取得
higedura 0:72e0bf0dbe7c 101 // if pos 初期値(0,0) -> noVzかも
higedura 0:72e0bf0dbe7c 102
higedura 0:72e0bf0dbe7c 103 // Get RC trim
higedura 0:72e0bf0dbe7c 104 for(int i=0;i<10;i++){
higedura 0:72e0bf0dbe7c 105 wait(.1);
higedura 0:72e0bf0dbe7c 106 PULTH_WIDTH_RC_TRIM_CH2 += g_pwm_pulse[0];
higedura 0:72e0bf0dbe7c 107 PULTH_WIDTH_RC_TRIM_CH3 += g_pwm_pulse[1];
higedura 0:72e0bf0dbe7c 108 }
higedura 0:72e0bf0dbe7c 109 PULTH_WIDTH_RC_TRIM_CH2 = PULTH_WIDTH_RC_TRIM_CH2 / 10;
higedura 0:72e0bf0dbe7c 110 PULTH_WIDTH_RC_TRIM_CH3 = PULTH_WIDTH_RC_TRIM_CH3 / 10;
higedura 0:72e0bf0dbe7c 111
higedura 0:72e0bf0dbe7c 112 // Check RC trim
higedura 0:72e0bf0dbe7c 113 if(PULTH_WIDTH_RC_TRIM_CH2 < 450 || 550 < PULTH_WIDTH_RC_TRIM_CH2){
higedura 0:72e0bf0dbe7c 114 //pc.printf("%d\r\n", PULTH_WIDTH_RC_TRIM_CH2);
higedura 0:72e0bf0dbe7c 115 while(1){
higedura 0:72e0bf0dbe7c 116 led2 = 1;
higedura 0:72e0bf0dbe7c 117 led3 = 0;
higedura 0:72e0bf0dbe7c 118 wait(0.5);
higedura 0:72e0bf0dbe7c 119 led2 = 0;
higedura 0:72e0bf0dbe7c 120 led3 = 1;
higedura 0:72e0bf0dbe7c 121 wait(0.5);
higedura 0:72e0bf0dbe7c 122 }
higedura 0:72e0bf0dbe7c 123 }
higedura 0:72e0bf0dbe7c 124
higedura 0:72e0bf0dbe7c 125 //////////////////// Stand by ////////////////////
higedura 0:72e0bf0dbe7c 126
higedura 0:72e0bf0dbe7c 127 ch2_in.disable_irq();
higedura 0:72e0bf0dbe7c 128 ch3_in.disable_irq();
higedura 0:72e0bf0dbe7c 129 ch5_in.disable_irq();
higedura 0:72e0bf0dbe7c 130
higedura 0:72e0bf0dbe7c 131 while(button < 0.5){
higedura 0:72e0bf0dbe7c 132 led1 = 1;
higedura 0:72e0bf0dbe7c 133 led4 = 1;
higedura 0:72e0bf0dbe7c 134 wait(0.5);
higedura 0:72e0bf0dbe7c 135 led1 = 0;
higedura 0:72e0bf0dbe7c 136 led4 = 0;
higedura 0:72e0bf0dbe7c 137 wait(0.5);
higedura 0:72e0bf0dbe7c 138 }
higedura 0:72e0bf0dbe7c 139
higedura 0:72e0bf0dbe7c 140 wait(3);
higedura 0:72e0bf0dbe7c 141
higedura 0:72e0bf0dbe7c 142 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
higedura 0:72e0bf0dbe7c 143
higedura 0:72e0bf0dbe7c 144 led1 = 1;
higedura 0:72e0bf0dbe7c 145 led4 = 1;
higedura 0:72e0bf0dbe7c 146
higedura 0:72e0bf0dbe7c 147 while(button < 0.5){
higedura 0:72e0bf0dbe7c 148 //while(1){
higedura 0:72e0bf0dbe7c 149
higedura 0:72e0bf0dbe7c 150 for(int i=0;i<5;i++){
higedura 0:72e0bf0dbe7c 151
higedura 0:72e0bf0dbe7c 152 //////////////////// Get PWM ////////////////////
higedura 0:72e0bf0dbe7c 153 ch2_in.enable_irq();
higedura 0:72e0bf0dbe7c 154 ch3_in.enable_irq();
higedura 0:72e0bf0dbe7c 155 ch5_in.enable_irq();
higedura 0:72e0bf0dbe7c 156
higedura 0:72e0bf0dbe7c 157 wait(.02); // Interval to get pwm
higedura 0:72e0bf0dbe7c 158
higedura 0:72e0bf0dbe7c 159 ch2_in.disable_irq();
higedura 0:72e0bf0dbe7c 160 ch3_in.disable_irq();
higedura 0:72e0bf0dbe7c 161 ch5_in.disable_irq();
higedura 0:72e0bf0dbe7c 162
higedura 0:72e0bf0dbe7c 163 //////////////////// Get pos and vel ////////////////////
higedura 0:72e0bf0dbe7c 164 if(xbee.readable()){
higedura 0:72e0bf0dbe7c 165 // get vz data
higedura 0:72e0bf0dbe7c 166 int buf_char[SIZE_DATA] = {0};
higedura 0:72e0bf0dbe7c 167
higedura 0:72e0bf0dbe7c 168 // decode vz data
higedura 0:72e0bf0dbe7c 169 for(int j=0;j<SIZE_DATA;j++){ buf_char[j] = (int)xbee.getc() - 48; }
higedura 0:72e0bf0dbe7c 170
higedura 0:72e0bf0dbe7c 171 //for(int i=0;i<SIZE_DATA;i++){ pc.printf("%5d ", buf_char[i]); }
higedura 0:72e0bf0dbe7c 172 //pc.printf("\r\n");
higedura 0:72e0bf0dbe7c 173
higedura 0:72e0bf0dbe7c 174 pos[1] = 1000*buf_char[0] + 100*buf_char[1] + 10*buf_char[2] + buf_char[3] - 5000;
higedura 0:72e0bf0dbe7c 175 pos[0] = 1000*buf_char[4] + 100*buf_char[5] + 10*buf_char[6] + buf_char[7] - 5000;
higedura 0:72e0bf0dbe7c 176
higedura 0:72e0bf0dbe7c 177 vel[1] = 1000* buf_char[8] + 100* buf_char[9] + 10*buf_char[10] + buf_char[11] - 5000;
higedura 0:72e0bf0dbe7c 178 vel[0] = 1000*buf_char[12] + 100*buf_char[13] + 10*buf_char[14] + buf_char[15] - 5000;
higedura 0:72e0bf0dbe7c 179 }else{
higedura 0:72e0bf0dbe7c 180
higedura 0:72e0bf0dbe7c 181 // しばらくこなかったらautoにならないフラグとか
higedura 0:72e0bf0dbe7c 182
higedura 0:72e0bf0dbe7c 183 }
higedura 0:72e0bf0dbe7c 184
higedura 0:72e0bf0dbe7c 185 //////////////////// Control law ////////////////////
higedura 0:72e0bf0dbe7c 186 if(500 < g_pwm_pulse[2]){
higedura 0:72e0bf0dbe7c 187
higedura 0:72e0bf0dbe7c 188 // Constant velocity F/B
higedura 0:72e0bf0dbe7c 189
higedura 0:72e0bf0dbe7c 190 //pulse_width_ch2_out = 0.001 + 0.000001 * g_pwm_pulse[0];
higedura 0:72e0bf0dbe7c 191 pulse_width_ch2_out = 0.001 + 0.000001 * PULTH_WIDTH_RC_TRIM_CH2 + (-1) * 0.000001 * GAIN_VEL_X * (VEL_X_TARGET - vel[0]);
higedura 0:72e0bf0dbe7c 192
higedura 0:72e0bf0dbe7c 193 pulse_width_ch3_out = 0.001 + 0.000001 * g_pwm_pulse[1];
higedura 0:72e0bf0dbe7c 194 //pulse_width_ch3_out = 0.001 + PULTH_WIDTH_RC_TRIM_CH3 + GAIN_VEL_Z * (-vel[1]);
higedura 0:72e0bf0dbe7c 195
higedura 0:72e0bf0dbe7c 196 }else{
higedura 0:72e0bf0dbe7c 197
higedura 0:72e0bf0dbe7c 198 // without RC-hack
higedura 0:72e0bf0dbe7c 199 pulse_width_ch2_out = 0.001 + 0.000001 * g_pwm_pulse[0];
higedura 0:72e0bf0dbe7c 200 pulse_width_ch3_out = 0.001 + 0.000001 * g_pwm_pulse[1];
higedura 0:72e0bf0dbe7c 201
higedura 0:72e0bf0dbe7c 202 }
higedura 0:72e0bf0dbe7c 203
higedura 0:72e0bf0dbe7c 204 // Limmiter for RC
higedura 0:72e0bf0dbe7c 205 if(pulse_width_ch2_out < LIMIT_PULTH_WIDTH_CH2_OUT_LOW){ pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_LOW; }
higedura 0:72e0bf0dbe7c 206 if(LIMIT_PULTH_WIDTH_CH2_OUT_UP < pulse_width_ch2_out) { pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_UP; }
higedura 0:72e0bf0dbe7c 207
higedura 0:72e0bf0dbe7c 208 ch2_out.pulsewidth(pulse_width_ch2_out);
higedura 0:72e0bf0dbe7c 209 ch3_out.pulsewidth(pulse_width_ch3_out);
higedura 0:72e0bf0dbe7c 210
higedura 0:72e0bf0dbe7c 211 }
higedura 0:72e0bf0dbe7c 212
higedura 0:72e0bf0dbe7c 213
higedura 0:72e0bf0dbe7c 214 //for(int i=0;i<N_CHAN;i++){ pc.printf("%5d ", g_pwm_pulse[i]); }
higedura 0:72e0bf0dbe7c 215 /*
higedura 0:72e0bf0dbe7c 216 pc.printf(" ");
higedura 0:72e0bf0dbe7c 217 for(int i=0;i<2;i++){ pc.printf("%5d ", pos[i]); }
higedura 0:72e0bf0dbe7c 218 for(int i=0;i<2;i++){ pc.printf("%5d ", vel[i]); }
higedura 0:72e0bf0dbe7c 219 pc.printf("\r\n");
higedura 0:72e0bf0dbe7c 220 */
higedura 0:72e0bf0dbe7c 221 fprintf(fp, "%f %f %5d %5d %5d %5d %5d \r\n", pulse_width_ch2_out, pulse_width_ch3_out, g_pwm_pulse[2], pos[0], pos[1], vel[0], vel[1]);
higedura 0:72e0bf0dbe7c 222
higedura 0:72e0bf0dbe7c 223 }
higedura 0:72e0bf0dbe7c 224
higedura 0:72e0bf0dbe7c 225 ch2_out.pulsewidth(0.0015);
higedura 0:72e0bf0dbe7c 226 ch3_out.pulsewidth(0.0011);
higedura 0:72e0bf0dbe7c 227
higedura 0:72e0bf0dbe7c 228 ch2_in.disable_irq();
higedura 0:72e0bf0dbe7c 229 ch3_in.disable_irq();
higedura 0:72e0bf0dbe7c 230 ch5_in.disable_irq();
higedura 0:72e0bf0dbe7c 231
higedura 0:72e0bf0dbe7c 232 fclose(fp);
higedura 0:72e0bf0dbe7c 233
higedura 0:72e0bf0dbe7c 234 led1 = 0;
higedura 0:72e0bf0dbe7c 235 led4 = 0;
higedura 0:72e0bf0dbe7c 236 led2 = 1;
higedura 0:72e0bf0dbe7c 237 led3 = 1;
higedura 0:72e0bf0dbe7c 238 }
higedura 0:72e0bf0dbe7c 239
higedura 0:72e0bf0dbe7c 240
higedura 0:72e0bf0dbe7c 241 //////////////////// RC interrupt (up) ////////////////////
higedura 0:72e0bf0dbe7c 242 //void roll_up(){ t_roll.start(); }
higedura 0:72e0bf0dbe7c 243 void pitch_up(){ t_pitch.start(); }
higedura 0:72e0bf0dbe7c 244 void thr_up(){ t_thr.start(); }
higedura 0:72e0bf0dbe7c 245 //void yaw_up(){ t_yaw.start(); }
higedura 0:72e0bf0dbe7c 246 void aux1_up(){ t_aux1.start(); }
higedura 0:72e0bf0dbe7c 247 //void aux2_up(){ t_aux2.start(); }
higedura 0:72e0bf0dbe7c 248
higedura 0:72e0bf0dbe7c 249
higedura 0:72e0bf0dbe7c 250 //////////////////// RC interrupt (down) ////////////////////
higedura 0:72e0bf0dbe7c 251 /*
higedura 0:72e0bf0dbe7c 252 void roll_down(){
higedura 0:72e0bf0dbe7c 253 t_roll.stop();
higedura 0:72e0bf0dbe7c 254 g_pwm_pulse[] = (int)(t_roll.read()*1000000-1000);
higedura 0:72e0bf0dbe7c 255 t_roll.reset();
higedura 0:72e0bf0dbe7c 256 }
higedura 0:72e0bf0dbe7c 257 */
higedura 0:72e0bf0dbe7c 258 void pitch_down(){
higedura 0:72e0bf0dbe7c 259 t_pitch.stop();
higedura 0:72e0bf0dbe7c 260 g_pwm_pulse[0] = (int)(t_pitch.read()*1000000-1000);
higedura 0:72e0bf0dbe7c 261 t_pitch.reset();
higedura 0:72e0bf0dbe7c 262 }
higedura 0:72e0bf0dbe7c 263
higedura 0:72e0bf0dbe7c 264 void thr_down(){
higedura 0:72e0bf0dbe7c 265 t_thr.stop();
higedura 0:72e0bf0dbe7c 266 g_pwm_pulse[1] = (int)(t_thr.read()*1000000-1000);
higedura 0:72e0bf0dbe7c 267 t_thr.reset();
higedura 0:72e0bf0dbe7c 268 }
higedura 0:72e0bf0dbe7c 269 /*
higedura 0:72e0bf0dbe7c 270 void yaw_down(){
higedura 0:72e0bf0dbe7c 271 t_yaw.stop();
higedura 0:72e0bf0dbe7c 272 g_pwm_pulse[] = (int)(t_yaw.read()*1000000-1000);
higedura 0:72e0bf0dbe7c 273 t_yaw.reset();
higedura 0:72e0bf0dbe7c 274 }
higedura 0:72e0bf0dbe7c 275 */
higedura 0:72e0bf0dbe7c 276 void aux1_down(){
higedura 0:72e0bf0dbe7c 277 t_aux1.stop();
higedura 0:72e0bf0dbe7c 278 g_pwm_pulse[2] = (int)(t_aux1.read()*1000000-1000);
higedura 0:72e0bf0dbe7c 279 t_aux1.reset();
higedura 0:72e0bf0dbe7c 280 }
higedura 0:72e0bf0dbe7c 281 /*
higedura 0:72e0bf0dbe7c 282 void aux2_down(){
higedura 0:72e0bf0dbe7c 283 t_aux2.stop();
higedura 0:72e0bf0dbe7c 284 g_pwm_pulse[] = (int)(t_aux2.read()*1000000-1000);
higedura 0:72e0bf0dbe7c 285 t_aux2.reset();
higedura 0:72e0bf0dbe7c 286 }
higedura 0:72e0bf0dbe7c 287 */