main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "stdlib.h"
00004 #include "main.h"
00005 #include "QEI.h"
00006 #include "TextLCD.h"
00007 
00008 int state[2] = {0}, rope_step = 0, waiting = 0, led_check = 0;
00009 double rev_r, rev_l, rev_p, pp = 0, rope_p = 0, power = 0, rope_s = 0;
00010 
00011 //超音波読み取り（割り込み）
00012 void ping()
00013 {
00014     Ultrasonic();
00015 }
00016 
00017 //回転速度読み取り（割り込み）
00018 void speed()
00019 {
00020 /*    static double pulse1 = 0, pulse2 = 0;
00021     double test;
00022     
00023     pulse2 = pulse1;
00024     pulse1 = fabs((double)enc_p.getPulses()/(360*4));
00025     
00026     rev_p = ave(2, pulse1 - pulse2);
00027 */    
00028     rev_l = ave(0, fabs((double)enc_l.getPulses())/(360*4));
00029     rev_r = ave(1, fabs((double)enc_r.getPulses())/(360*4));
00030     rev_p = ave(2, fabs((double)enc_p.getPulses())/(360*4));
00031     
00032     //printf("%03.4f   %03.4f    %f\n", rev_l, rev_r, pp);
00033 /*    
00034     lcd.locate(2, 0);
00035     lcd.printf("%1.5f", rope_p);
00036     //lcd.printf("%03.4f", rev_l);
00037     lcd.locate(0, 1);
00038     lcd.printf("%03.4f", rev_p);
00039 */
00040 /*    lcd.cls();
00041     lcd.locate(2, 0);
00042     lcd.printf("%d", enc_p.getPulses()/2/320);
00043     lcd.locate(0, 1);
00044     lcd.printf("%d", enc_p.getPulses());
00045 */
00046     
00047     enc_l.reset();
00048     enc_r.reset();
00049     enc_p.reset();
00050 }
00051 
00052 //p制御
00053 double pid(double target, double sensor, double kp)
00054 {
00055     double diff, p;
00056     
00057     diff = target - sensor;
00058     
00059     p = kp * diff;
00060     
00061     return p;
00062 }
00063 
00064 void ppp()
00065 {
00066 /*    if(state[WHEEL]){
00067         pp += pid((rev_l - 0.01), rev_r, 0.05);
00068         pwm[1] = pp;
00069     }
00070 */    
00071     if(rope_s){
00072         rope_p += pid(ROPE_S, rev_p, 0.001);
00073         if(rope_p > ROPE_MAX){
00074             rope_p = ROPE_MAX;
00075         } else if(rope_p < ROPE_V){
00076             rope_p = ROPE_V;
00077         }
00078         pwm[2] = rope_p;
00079     }
00080 }
00081 
00082 void rope_steps()
00083 {
00084     if(rope_step == 0){
00085         pwm[2] = ROPE_V;
00086         rope_s = 1;
00087         state[ROPE] = 1;
00088         rope_step++;
00089     } else if(rope_step == 1){
00090         pwm[2] = ROPE_L;
00091         rope_s = 0;
00092         rope_steper.attach(&rope_steps,0.8);
00093         rope_step++;
00094     } else {
00095         rope = 0;
00096         legs = 0;
00097         state[ROPE] = 0;
00098         rope_step = 0;
00099     }
00100  /*   
00101     lcd.locate(0, 0);
00102     lcd.printf("%d", rope_step);
00103     */
00104 }
00105 
00106 void rope_wait()
00107 {
00108     waiting = 0;
00109 }
00110 
00111 void wheel_stop()
00112 {
00113     wheel = 0;
00114 }
00115 /*
00116 void ledwait()
00117 {
00118     led_wait = 0;
00119 }
00120 */
00121 /*
00122 void warikomi()
00123 {
00124     static int rev = 0;
00125     
00126     rgbled = 0x2A;
00127     wait(0.1);
00128     rgbled = 0x15;
00129     
00130     rev++;
00131     if(rev > 1){
00132         myled[1] = 0;
00133         rope_steps();
00134         waiter.attach(&rope_wait,1);
00135         waiting = 1;
00136         rev = 0;
00137     }
00138     
00139     //wait(0.1);
00140 }
00141 */
00142 int main() {
00143 
00144     int rev = 0;
00145     double s_flag[4] = {0};
00146     
00147     timer2.start();
00148     pinger.attach(&ping,0.1);
00149     pider.attach(&ppp,0.01);
00150     speeder.attach(&speed,0.1);
00151     //sw.mode(PullUp);
00152     
00153     //sw.fall(&warikomi);
00154     
00155     pwm[0] = WHEEL_V;
00156     pwm[1] = WHEEL_VL;
00157     pwm[3] = 0.1;
00158     pp = WHEEL_V;
00159     rope_p = ROPE_V;
00160     //pwm[1] = 0.2;
00161     
00162     start_led();
00163     
00164 /*   
00165     for(;;){
00166         pc.printf("%05d  %05d  %05d  %05d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]);
00167         wait(0.1);
00168     }
00169 */
00170     
00171     while(1) {
00172         //data = ave(0, ultrasonicVal[0]);
00173         
00174         //pc.printf("%06d  %06d  %06d  %06d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]);
00175         
00176         if(ultrasonicVal[0] < TYONPA_S){
00177             s_flag[0]++;
00178         } else {
00179             s_flag[0] = 0;
00180         }
00181         
00182         if(ultrasonicVal[1] < TYONPA_S){
00183             s_flag[1]++;
00184         } else {
00185             s_flag[1] = 0;
00186         }
00187         
00188         if(ultrasonicVal[2] < TYONPA_U){
00189             s_flag[2]++;
00190         } else {
00191             s_flag[2] = 0;
00192         }
00193         
00194         if(ultrasonicVal[3] < TYONPA_U){
00195             s_flag[3]++;
00196         } else {
00197             s_flag[3] = 0;
00198         }
00199         
00200         
00201         if(s_flag[0] > FLAG_W){
00202             if(state[WHEEL] == 0){
00203                 wheel_stoper.detach();
00204             }
00205             myled[3] = 1;
00206             state[WHEEL] = 1;
00207             pwm[0] = WHEEL_V;
00208             pwm[1] = WHEEL_VL;
00209             wheel = 0x05;
00210             rgbled = 0;
00211         } else if(s_flag[1] > FLAG_W){
00212             if(state[WHEEL] == 0){
00213                 wheel_stoper.detach();
00214             }
00215             myled[2] = 1;
00216             state[WHEEL] = 1;
00217             pwm[0] = WHEEL_V;
00218             pwm[1] = WHEEL_VL;
00219             wheel = 0x0A;
00220             rgbled = 0;
00221         } else {
00222             if(state[WHEEL]){
00223                 wheel_stoper.attach(&wheel_stop,0.3);
00224                 state[WHEEL] = 0;
00225                 pwm[0] = 0;
00226                 pwm[1] = 0;
00227                 myled[3] = 0;
00228                 myled[2] = 0;
00229             }
00230             //wheel = 0;
00231             rgbled = 0x15;
00232         }
00233         
00234         if(s_flag[2] > FLAG_R){
00235             if(waiting == 0){
00236                 if(state[ROPE] == 0){
00237                     myled[1] = 1;
00238                     rope = 1;
00239                     pwm[2] = ROPE_V0;
00240                     rope_steper.attach(&rope_steps,1.5);
00241                     waiter.attach(&rope_wait,1);
00242                     legs = 0x02;
00243                     //wait(1);
00244                 } else {
00245                     myled[1] = 0;
00246                     rope_steps();
00247                     waiter.attach(&rope_wait,1);
00248                 }
00249                 waiting = 1;
00250             }
00251         }
00252         
00253         if(s_flag[3] > FLAG_R){
00254             rgbled = 0x2A;
00255             wait(0.1);
00256             rgbled = 0x15;
00257         }
00258         
00259         if(sw == 0){
00260             rgbled = 0x2A;
00261             wait(0.1);
00262             if(led_check == 0){
00263                 rev++;
00264                 led_check = 1;
00265                 //led_wait = 1;
00266                 //waiter.attach(&ledwait,0.2);
00267                 if(rev > 1){
00268                     myled[1] = 0;
00269                     rope_steps();
00270                     waiter.attach(&rope_wait,1);
00271                     waiting = 1;
00272                     rev = 0;
00273                 }
00274             }
00275         } else {
00276             led_check = 0;
00277         }
00278 /*        
00279         if((s_flag[0] > FLAG_W) || (s_flag[1] > FLAG_W)){
00280             rgbled = 0x09;
00281         } else if((sw == 0) || (s_flag[3] > FLAG_R)){
00282             rgbled = 0x24;
00283         } else {
00284             rgbled = 0x12;
00285             //0000000.wait(0.1);
00286         }
00287 */        
00288     }
00289 }