Takujyou_Ishii / Mbed 2 deprecated isiiken

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SpeedController passfollowing

main.cpp@0:d56593bc83cc, 2021-02-27 (annotated)

Committer:
aoikoizumi
Date:
Sat Feb 27 07:38:58 2021 +0000
Revision:
0:d56593bc83cc
Child:
1:3639b161473e
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:d56593bc83cc 1 //CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
aoikoizumi 0:d56593bc83cc 2 #include "mbed.h"
aoikoizumi 0:d56593bc83cc 3 #include "EC.h" //Encoderライブラリをインクルード
aoikoizumi 0:d56593bc83cc 4 #include "SpeedController.h" //SpeedControlライブラリをインクルード
aoikoizumi 0:d56593bc83cc 5 #include "math.h"
aoikoizumi 0:d56593bc83cc 6 #define RESOLUTION 400 //分解能
aoikoizumi 0:d56593bc83cc 7 #define BASIC_SPEED 40 //動確用
aoikoizumi 0:d56593bc83cc 8
aoikoizumi 0:d56593bc83cc 9 //#define TEST_DUTY 0.3 //動確用
aoikoizumi 0:d56593bc83cc 10
aoikoizumi 0:d56593bc83cc 11
aoikoizumi 0:d56593bc83cc 12 Ec4multi EC_1(PA_8,PA_11,RESOLUTION);
aoikoizumi 0:d56593bc83cc 13 Ec4multi EC_2(PB_5,PB_4,RESOLUTION);
aoikoizumi 0:d56593bc83cc 14 //Ec4multi EC_1(p15,p16,RESOLUTION);
aoikoizumi 0:d56593bc83cc 15 //Ec4multi EC_2(p17,p18,RESOLUTION);
aoikoizumi 0:d56593bc83cc 16 SpeedControl motor_1(PA_10,PB_7,50,EC_1);
aoikoizumi 0:d56593bc83cc 17 SpeedControl motor_2(PA_12,PB_6,50,EC_2);
aoikoizumi 0:d56593bc83cc 18 PwmOut pump(PA_7);
aoikoizumi 0:d56593bc83cc 19 AnalogIn a_in(PA_4);
aoikoizumi 0:d56593bc83cc 20 AnalogIn b_in(PA_3);
aoikoizumi 0:d56593bc83cc 21 AnalogIn c_in(PA_1);
aoikoizumi 0:d56593bc83cc 22 AnalogIn d_in(PA_0);
aoikoizumi 0:d56593bc83cc 23
aoikoizumi 0:d56593bc83cc 24 //SpeedControl motor_1(p21,p22,50,EC_1);
aoikoizumi 0:d56593bc83cc 25 //SpeedControl motor_2(p24,p23,50,EC_2);
aoikoizumi 0:d56593bc83cc 26
aoikoizumi 0:d56593bc83cc 27 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:d56593bc83cc 28 Serial pc(USBTX,USBRX);
aoikoizumi 0:d56593bc83cc 29
aoikoizumi 0:d56593bc83cc 30 Timer timer;
aoikoizumi 0:d56593bc83cc 31
aoikoizumi 0:d56593bc83cc 32 int j=0;
aoikoizumi 0:d56593bc83cc 33 int k = 0;
aoikoizumi 0:d56593bc83cc 34 double rotate_1=0,rotate_2=0;
aoikoizumi 0:d56593bc83cc 35 double mt_out1=0,mt_out2=0;
aoikoizumi 0:d56593bc83cc 36 double tire_fai_mm=88;//タイヤ径
aoikoizumi 0:d56593bc83cc 37 double machine_width=90.6;//機体の横幅
aoikoizumi 0:d56593bc83cc 38 double odm_x=0;
aoikoizumi 0:d56593bc83cc 39 double odm_y=0;
aoikoizumi 0:d56593bc83cc 40 double odm_theta=0;
aoikoizumi 0:d56593bc83cc 41 double odm_righttire=0;
aoikoizumi 0:d56593bc83cc 42 double odm_lefttire=0;
aoikoizumi 0:d56593bc83cc 43 double old_rightrad=0;
aoikoizumi 0:d56593bc83cc 44 double old_leftrad=0;
aoikoizumi 0:d56593bc83cc 45 double old_theta=0;
aoikoizumi 0:d56593bc83cc 46 void passfollowing(double start_x,double start_y,double tgt_x,double tgt_y,double tgt_theta);
aoikoizumi 0:d56593bc83cc 47 //double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
aoikoizumi 0:d56593bc83cc 48 //int motor1_count[1000]= {},motor2_count[1000]= {};
aoikoizumi 0:d56593bc83cc 49
aoikoizumi 0:d56593bc83cc 50 void CalOmega()
aoikoizumi 0:d56593bc83cc 51 {
aoikoizumi 0:d56593bc83cc 52
aoikoizumi 0:d56593bc83cc 53 EC_1.calOmega();
aoikoizumi 0:d56593bc83cc 54 EC_2.calOmega();
aoikoizumi 0:d56593bc83cc 55 odm_righttire=tire_fai_mm*(EC_1.getRad()-old_rightrad)/2;
aoikoizumi 0:d56593bc83cc 56 odm_lefttire=tire_fai_mm*(EC_2.getRad()-old_leftrad)/2;//÷2するのが正しいが切ると正しくなる()
aoikoizumi 0:d56593bc83cc 57 odm_theta+=atan(odm_righttire-odm_lefttire)/machine_width;
aoikoizumi 0:d56593bc83cc 58 odm_x+=-(odm_righttire+odm_lefttire)*sin((odm_theta+old_theta)/2)/2;
aoikoizumi 0:d56593bc83cc 59 odm_y+=(odm_righttire+odm_lefttire)*cos((odm_theta+old_theta)/2)/2;
aoikoizumi 0:d56593bc83cc 60 if(rotate_1==0) motor_1.stop();
aoikoizumi 0:d56593bc83cc 61 else motor_1.Sc(rotate_1);
aoikoizumi 0:d56593bc83cc 62 if(rotate_2==0) motor_2.stop();
aoikoizumi 0:d56593bc83cc 63 else motor_2.Sc(rotate_2);
aoikoizumi 0:d56593bc83cc 64 old_rightrad=EC_1.getRad();
aoikoizumi 0:d56593bc83cc 65 old_leftrad=EC_2.getRad();
aoikoizumi 0:d56593bc83cc 66 old_theta=odm_theta;
aoikoizumi 0:d56593bc83cc 67 j++;
aoikoizumi 0:d56593bc83cc 68 //printf("%d ",k%100);
aoikoizumi 0:d56593bc83cc 69 //printf("%f,%f/r/n",rotate_1,rotate_1);
aoikoizumi 0:d56593bc83cc 70 }
aoikoizumi 0:d56593bc83cc 71 double calcdif(double start_x,double start_y,double tgt_x,double tgt_y,double tgt_theta,int separation);
aoikoizumi 0:d56593bc83cc 72 void sensor_run(int form_or_back,double max_speed);
aoikoizumi 0:d56593bc83cc 73 void gogogo_straight(double start_x,double start_y,double tgt_x,double tgt_y,double tgt_theta,double max_speed);
aoikoizumi 0:d56593bc83cc 74 void turnAtThePoint( double start_theta, double tgt_theta, double max_speed);
aoikoizumi 0:d56593bc83cc 75
aoikoizumi 0:d56593bc83cc 76
aoikoizumi 0:d56593bc83cc 77 int main()
aoikoizumi 0:d56593bc83cc 78 {
aoikoizumi 0:d56593bc83cc 79 //UserLoopSetting_can();
aoikoizumi 0:d56593bc83cc 80
aoikoizumi 0:d56593bc83cc 81
aoikoizumi 0:d56593bc83cc 82 motor_1.period_us(50);
aoikoizumi 0:d56593bc83cc 83 motor_2.period_us(50);
aoikoizumi 0:d56593bc83cc 84 pump.period_us(50);
aoikoizumi 0:d56593bc83cc 85 //motor_1.setEquation(0.0429,0,0.0429,0); //求めたC,Dの値を設定
aoikoizumi 0:d56593bc83cc 86 //motor_2.setEquation(0.0369,0,0.0429,0); //求めたC,Dの値を設定
aoikoizumi 0:d56593bc83cc 87 motor_1.setEquation(0.04,0.4,0.04,0.4); //求めたC,Dの値を設定
aoikoizumi 0:d56593bc83cc 88 motor_2.setEquation(0.04,0.4,0.04,0.4); //求めたC,Dの値を設定
aoikoizumi 0:d56593bc83cc 89 motor_1.setPDparam(0.05,0.0); //PIDの係数を設定
aoikoizumi 0:d56593bc83cc 90 motor_2.setPDparam(0.05,0.0); //PIDの係数を設定
aoikoizumi 0:d56593bc83cc 91 motor_1.setDutyLimit(0.95);
aoikoizumi 0:d56593bc83cc 92 motor_2.setDutyLimit(0.95);
aoikoizumi 0:d56593bc83cc 93 motor_tick.attach(&CalOmega,0.005);
aoikoizumi 0:d56593bc83cc 94
aoikoizumi 0:d56593bc83cc 95 rotate_1=0;
aoikoizumi 0:d56593bc83cc 96 rotate_2=0;
aoikoizumi 0:d56593bc83cc 97 //printf("gs,%.3lf,%.3lf,%.3lf ,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf,z=%.3lf\r\n",EC_1.getOmega(),EC_1.getRad(),rotate_1,EC_2.getOmega(),EC_2.getRad(),rotate_2,odm_x,odm_y,odm_theta*180/3.141592);
aoikoizumi 0:d56593bc83cc 98
aoikoizumi 0:d56593bc83cc 99 wait(1);
aoikoizumi 0:d56593bc83cc 100 //printf("gs,%.3lf %.3lf %.3lf ,%.3lf %.3lf %.3lf,%.3lf,%.3lf,z=%.3lf\r\n",EC_1.getOmega(),EC_1.getRad(),rotate_1,EC_2.getOmega(),EC_2.getRad(),rotate_2,odm_x,odm_y,odm_theta*180/3.141592);
aoikoizumi 0:d56593bc83cc 101 //gogogo_straight(0,0,0,200,0,5);//起点X起点Y終点X終点Y角度速度
aoikoizumi 0:d56593bc83cc 102 turnAtThePoint(0,90,4);
aoikoizumi 0:d56593bc83cc 103 //sensor_run(0,1);//前後,最高速
aoikoizumi 0:d56593bc83cc 104 rotate_1=0;
aoikoizumi 0:d56593bc83cc 105 rotate_2=0;
aoikoizumi 0:d56593bc83cc 106 wait(1);
aoikoizumi 0:d56593bc83cc 107 while(1) {
aoikoizumi 0:d56593bc83cc 108 //pump=1;
aoikoizumi 0:d56593bc83cc 109 //printf("done");
aoikoizumi 0:d56593bc83cc 110 //rotate_1 = mt_out1;
aoikoizumi 0:d56593bc83cc 111 //rotate_2 = mt_out2;
aoikoizumi 0:d56593bc83cc 112
aoikoizumi 0:d56593bc83cc 113 if(pc.readable()) {
aoikoizumi 0:d56593bc83cc 114 char sel=pc.getc();
aoikoizumi 0:d56593bc83cc 115 if(sel=='s') {
aoikoizumi 0:d56593bc83cc 116 motor_1.stop();
aoikoizumi 0:d56593bc83cc 117 motor_2.stop();
aoikoizumi 0:d56593bc83cc 118 rotate_1=0;
aoikoizumi 0:d56593bc83cc 119 rotate_2=0;
aoikoizumi 0:d56593bc83cc 120 } else if(sel=='j') {
aoikoizumi 0:d56593bc83cc 121 rotate_1+=1; //eを押すとduty比がo.o2ずつあがる
aoikoizumi 0:d56593bc83cc 122 pc.printf("tgt=%f\r\n",rotate_1);
aoikoizumi 0:d56593bc83cc 123 } else if(sel=='u') {
aoikoizumi 0:d56593bc83cc 124 rotate_2+=1; //eを押すとduty比がo.o2ずつあがる
aoikoizumi 0:d56593bc83cc 125 pc.printf("tgt=%f\r\n",rotate_2);
aoikoizumi 0:d56593bc83cc 126 } else if(sel=='k') {
aoikoizumi 0:d56593bc83cc 127 rotate_1-=1; //dを押すとduty比がo.o2ずつさがる
aoikoizumi 0:d56593bc83cc 128 pc.printf("tgt=%f\r\n",rotate_1);
aoikoizumi 0:d56593bc83cc 129 } else if(sel=='i') {
aoikoizumi 0:d56593bc83cc 130 rotate_2-=1; //dを押すとduty比がo.o2ずつさがる
aoikoizumi 0:d56593bc83cc 131 pc.printf("tgt=%f\r\n",rotate_2);
aoikoizumi 0:d56593bc83cc 132 //else if(sel=='p'){
aoikoizumi 0:d56593bc83cc 133 // print=!print; //pを押すと表示を停止/再開する
aoikoizumi 0:d56593bc83cc 134 //}
aoikoizumi 0:d56593bc83cc 135 }
aoikoizumi 0:d56593bc83cc 136 }
aoikoizumi 0:d56593bc83cc 137
aoikoizumi 0:d56593bc83cc 138
aoikoizumi 0:d56593bc83cc 139 if(k>100000) {
aoikoizumi 0:d56593bc83cc 140 printf("%.3lf %.3lf %.3lf ,%.3lf %.3lf %.3lf////x=%.3lf,y=%.3lf,z=%.3lf\r\n",EC_1.getOmega(),EC_1.getRad(),rotate_1,EC_2.getOmega(),EC_2.getRad(),rotate_2,odm_x,odm_y,odm_theta*180/3.141592);
aoikoizumi 0:d56593bc83cc 141 //printf("\nloop\n\n\r");
aoikoizumi 0:d56593bc83cc 142 //printf("%.3lf,%.3lf",motor_1.Sc.duty,motor_2.Sc.duty);
aoikoizumi 0:d56593bc83cc 143 k=0;
aoikoizumi 0:d56593bc83cc 144 }
aoikoizumi 0:d56593bc83cc 145 k++;
aoikoizumi 0:d56593bc83cc 146
aoikoizumi 0:d56593bc83cc 147 //printf("%d",k);
aoikoizumi 0:d56593bc83cc 148 }
aoikoizumi 0:d56593bc83cc 149 //printf("finish\n\r");
aoikoizumi 0:d56593bc83cc 150 }
aoikoizumi 0:d56593bc83cc 151 double start()
aoikoizumi 0:d56593bc83cc 152 {
aoikoizumi 0:d56593bc83cc 153 while(1) {
aoikoizumi 0:d56593bc83cc 154
aoikoizumi 0:d56593bc83cc 155 }
aoikoizumi 0:d56593bc83cc 156 }
aoikoizumi 0:d56593bc83cc 157 bool goal=false;
aoikoizumi 0:d56593bc83cc 158 double calcdif(double start_x,double start_y,double tgt_x,double tgt_y,double tgt_theta)
aoikoizumi 0:d56593bc83cc 159 {
aoikoizumi 0:d56593bc83cc 160 goal=false;
aoikoizumi 0:d56593bc83cc 161 double Vector_ab[2] = {(tgt_x - start_x), (tgt_y - start_y)}; //ベクトルAB
aoikoizumi 0:d56593bc83cc 162 double ab_Vector_size = hypot(Vector_ab[0], Vector_ab[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
aoikoizumi 0:d56593bc83cc 163 double UnitVector_ab[2] = {Vector_ab[0]/ab_Vector_size, Vector_ab[1]/ab_Vector_size}; //ベクトルABの単位ベクトル
aoikoizumi 0:d56593bc83cc 164 double UnitVector_ch[2] = {UnitVector_ab[1], -UnitVector_ab[0]}; //ベクトルCHの単位ベクトル
aoikoizumi 0:d56593bc83cc 165 double Vector_ac[2] = {(odm_x - start_x), (odm_y - start_y)}; //ベクトルAC
aoikoizumi 0:d56593bc83cc 166 double Vector_bc[2] = {(odm_x - tgt_x), (odm_y - tgt_y)}; //ベクトルBC
aoikoizumi 0:d56593bc83cc 167 double diff = UnitVector_ab[0]*Vector_ac[1] - UnitVector_ab[1]*Vector_ac[0]; //機体位置と直線ABの距離（外積を用いて計算）
aoikoizumi 0:d56593bc83cc 168 //printf("%f\r\n",Vector_ac[0]*Vector_bc[0]+Vector_ac[1]*Vector_bc[1]);
aoikoizumi 0:d56593bc83cc 169 if(Vector_ac[0]*Vector_bc[0]+Vector_ac[1]*Vector_bc[1]>0.01) {
aoikoizumi 0:d56593bc83cc 170 goal=true;
aoikoizumi 0:d56593bc83cc 171 }
aoikoizumi 0:d56593bc83cc 172 return(diff);
aoikoizumi 0:d56593bc83cc 173 }
aoikoizumi 0:d56593bc83cc 174 void gogogo_straight(double start_x,double start_y,double tgt_x,double tgt_y,double tgt_theta,double max_speed)
aoikoizumi 0:d56593bc83cc 175 {
aoikoizumi 0:d56593bc83cc 176 double path_p=0.001,path_d=0;
aoikoizumi 0:d56593bc83cc 177 //double angle_p=0.1,angle_d=0;
aoikoizumi 0:d56593bc83cc 178 double diff,diff_angle;
aoikoizumi 0:d56593bc83cc 179 double old_gggst;
aoikoizumi 0:d56593bc83cc 180 double old_diff;
aoikoizumi 0:d56593bc83cc 181 Timer gggst_timer;
aoikoizumi 0:d56593bc83cc 182 gggst_timer.start();
aoikoizumi 0:d56593bc83cc 183 while(1) {
aoikoizumi 0:d56593bc83cc 184 if(k>1000) {
aoikoizumi 0:d56593bc83cc 185
aoikoizumi 0:d56593bc83cc 186 printf("gs,%.3lf,%.3lf,%.3lf ,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf\r\n",EC_1.getOmega(),EC_1.getRad(),rotate_1,EC_2.getOmega(),EC_2.getRad(),rotate_2,odm_x,odm_y,odm_theta*180/3.141592);
aoikoizumi 0:d56593bc83cc 187 //printf("\nloop\n\n\r");
aoikoizumi 0:d56593bc83cc 188 k=0;
aoikoizumi 0:d56593bc83cc 189 }
aoikoizumi 0:d56593bc83cc 190 k++;
aoikoizumi 0:d56593bc83cc 191 diff_angle=odm_theta*180/3.141592-tgt_theta;
aoikoizumi 0:d56593bc83cc 192 diff=calcdif(start_x,start_y,tgt_x,tgt_y,tgt_theta);
aoikoizumi 0:d56593bc83cc 193 if(goal==true) {
aoikoizumi 0:d56593bc83cc 194 gggst_timer.stop();
aoikoizumi 0:d56593bc83cc 195 gggst_timer.reset();
aoikoizumi 0:d56593bc83cc 196 break;
aoikoizumi 0:d56593bc83cc 197 }//21
aoikoizumi 0:d56593bc83cc 198 /*
aoikoizumi 0:d56593bc83cc 199 if(diff>=5) {//右側、位置制御(粗い)
aoikoizumi 0:d56593bc83cc 200 rotate_1=max_speed-path_p*(diff);
aoikoizumi 0:d56593bc83cc 201 rotate_2=max_speed;
aoikoizumi 0:d56593bc83cc 202 //rotate_2=max_speed-path_d*(old_diff-diff)/(old_gggst-gggst_timer.read());
aoikoizumi 0:d56593bc83cc 203 } else if(diff>=0) {//右側、角度制御(細)
aoikoizumi 0:d56593bc83cc 204 rotate_1=max_speed+angle_p*(diff_angle);
aoikoizumi 0:d56593bc83cc 205 rotate_2=max_speed;
aoikoizumi 0:d56593bc83cc 206 //rotate_2=max_speed-path_d*(old_diff-diff)/(old_gggst-gggst_timer.read());
aoikoizumi 0:d56593bc83cc 207 } else if(diff>=-5){//左側、角度制御(細)
aoikoizumi 0:d56593bc83cc 208 //rotate_1=max_speed-path_d*(old_diff-diff)/(old_gggst-gggst_timer.read());
aoikoizumi 0:d56593bc83cc 209 rotate_1=max_speed;
aoikoizumi 0:d56593bc83cc 210 rotate_2=max_speed-angle_p*(diff_angle);
aoikoizumi 0:d56593bc83cc 211 }else {//左側、位置制御(粗い)
aoikoizumi 0:d56593bc83cc 212 //rotate_1=max_speed-path_d*(old_diff-diff)/(old_gggst-gggst_timer.read());
aoikoizumi 0:d56593bc83cc 213 rotate_1=max_speed;
aoikoizumi 0:d56593bc83cc 214 rotate_2=max_speed+path_p*(diff);
aoikoizumi 0:d56593bc83cc 215 }
aoikoizumi 0:d56593bc83cc 216 */
aoikoizumi 0:d56593bc83cc 217 if(diff*diff_angle<0) {
aoikoizumi 0:d56593bc83cc 218 rotate_1=max_speed-path_p*(diff)/2;
aoikoizumi 0:d56593bc83cc 219 rotate_2=max_speed+path_p*(diff)/2;
aoikoizumi 0:d56593bc83cc 220 } else {
aoikoizumi 0:d56593bc83cc 221 rotate_1=max_speed-path_p*(diff)*5;
aoikoizumi 0:d56593bc83cc 222 rotate_2=max_speed+path_p*(diff)*5;
aoikoizumi 0:d56593bc83cc 223 }
aoikoizumi 0:d56593bc83cc 224
aoikoizumi 0:d56593bc83cc 225 old_diff=diff;
aoikoizumi 0:d56593bc83cc 226 old_gggst=gggst_timer.read();
aoikoizumi 0:d56593bc83cc 227 }
aoikoizumi 0:d56593bc83cc 228 }
aoikoizumi 0:d56593bc83cc 229 void turnAtThePoint(double start_theta, double tgt_theta, double max_speed)
aoikoizumi 0:d56593bc83cc 230 {
aoikoizumi 0:d56593bc83cc 231 while(1) {
aoikoizumi 0:d56593bc83cc 232 double speed=(tgt_theta-odm_theta*180/3.141592)/20;
aoikoizumi 0:d56593bc83cc 233 if(speed<0) {
aoikoizumi 0:d56593bc83cc 234 speed=-speed;
aoikoizumi 0:d56593bc83cc 235 }
aoikoizumi 0:d56593bc83cc 236 if(speed>max_speed) {
aoikoizumi 0:d56593bc83cc 237 speed=max_speed;
aoikoizumi 0:d56593bc83cc 238 }
aoikoizumi 0:d56593bc83cc 239 if(odm_theta*180/3.141592>tgt_theta) {
aoikoizumi 0:d56593bc83cc 240 rotate_1=-speed;
aoikoizumi 0:d56593bc83cc 241 rotate_2=speed;
aoikoizumi 0:d56593bc83cc 242 } else {
aoikoizumi 0:d56593bc83cc 243 rotate_1=speed;
aoikoizumi 0:d56593bc83cc 244 rotate_2=-speed;
aoikoizumi 0:d56593bc83cc 245 }
aoikoizumi 0:d56593bc83cc 246 printf("%f,%f,%f,%f,\r\n",tgt_theta,odm_theta*180/3.141592,odm_x,odm_y);
aoikoizumi 0:d56593bc83cc 247 if(odm_theta*180/3.141592+2>tgt_theta&&odm_theta*180/3.141592-2<tgt_theta) {
aoikoizumi 0:d56593bc83cc 248 rotate_1=0;
aoikoizumi 0:d56593bc83cc 249 rotate_2=0;
aoikoizumi 0:d56593bc83cc 250 wait(0.5);
aoikoizumi 0:d56593bc83cc 251 if(odm_theta*180/3.141592+2>tgt_theta&&odm_theta*180/3.141592-2<tgt_theta) {
aoikoizumi 0:d56593bc83cc 252 break;
aoikoizumi 0:d56593bc83cc 253 }
aoikoizumi 0:d56593bc83cc 254 }
aoikoizumi 0:d56593bc83cc 255 }
aoikoizumi 0:d56593bc83cc 256 }
aoikoizumi 0:d56593bc83cc 257 void sensor_run(int form_or_back,double max_speed)
aoikoizumi 0:d56593bc83cc 258 {
aoikoizumi 0:d56593bc83cc 259
aoikoizumi 0:d56593bc83cc 260 Timer gggst_timer;
aoikoizumi 0:d56593bc83cc 261 gggst_timer.start();
aoikoizumi 0:d56593bc83cc 262 while(1) {
aoikoizumi 0:d56593bc83cc 263 if(form_or_back==0) {
aoikoizumi 0:d56593bc83cc 264 if((float)a_in<=(float)0.95) {
aoikoizumi 0:d56593bc83cc 265 rotate_2=max_speed;
aoikoizumi 0:d56593bc83cc 266 } else {
aoikoizumi 0:d56593bc83cc 267 rotate_2=0;
aoikoizumi 0:d56593bc83cc 268 }
aoikoizumi 0:d56593bc83cc 269 if((float)b_in<=(float)0.95) {
aoikoizumi 0:d56593bc83cc 270 rotate_1=max_speed;
aoikoizumi 0:d56593bc83cc 271 } else {
aoikoizumi 0:d56593bc83cc 272 rotate_1=0;
aoikoizumi 0:d56593bc83cc 273 }
aoikoizumi 0:d56593bc83cc 274 } else {
aoikoizumi 0:d56593bc83cc 275 if((float)c_in<=(float)0.95) {
aoikoizumi 0:d56593bc83cc 276 rotate_1=-max_speed;
aoikoizumi 0:d56593bc83cc 277 } else {
aoikoizumi 0:d56593bc83cc 278 rotate_1=0;
aoikoizumi 0:d56593bc83cc 279 }
aoikoizumi 0:d56593bc83cc 280 if((float)d_in<=(float)0.95) {
aoikoizumi 0:d56593bc83cc 281 rotate_2=-max_speed;
aoikoizumi 0:d56593bc83cc 282 } else {
aoikoizumi 0:d56593bc83cc 283 rotate_2=0;
aoikoizumi 0:d56593bc83cc 284 }
aoikoizumi 0:d56593bc83cc 285 }
aoikoizumi 0:d56593bc83cc 286 if(k>1000) {
aoikoizumi 0:d56593bc83cc 287 //printf("gs,%.3lf,%.3lf,%.3lf ,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf\r\n",EC_1.getOmega(),EC_1.getRad(),rotate_1,EC_2.getOmega(),EC_2.getRad(),rotate_2,odm_x,odm_y,odm_theta*180/3.141592);
aoikoizumi 0:d56593bc83cc 288 printf("loop\n\r");
aoikoizumi 0:d56593bc83cc 289 printf("%f,%f,%f,%f,%lf,%lf",(float)a_in,(float)b_in,(float)c_in,(float)d_in,rotate_1,rotate_2);
aoikoizumi 0:d56593bc83cc 290 k=0;
aoikoizumi 0:d56593bc83cc 291 }
aoikoizumi 0:d56593bc83cc 292 k++;
aoikoizumi 0:d56593bc83cc 293 if(rotate_1==0&&rotate_2==0) {
aoikoizumi 0:d56593bc83cc 294 break;
aoikoizumi 0:d56593bc83cc 295 }//21
aoikoizumi 0:d56593bc83cc 296 }
aoikoizumi 0:d56593bc83cc 297 }