NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2020BAuto

Dependencies:   mbed ikarashiMDC_2byte_ver lpf

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 //#include"IRsensor.h"
00003 #include "ikarashiMDC.h"
00004 //#include "hcsr04.h"
00005 #include "pin_config.h"
00006 #include "lpf.h"
00007 
00008 Serial pc(USBTX,USBRX,115200);
00009 DigitalOut Estop(D12);
00010 Serial serial(PC_10, PC_11, 115200);
00011 DigitalIn turnLimit(PB_3);
00012 //HCSR04  usensor(D9,D10);
00013 //IRsensor ir1(PC_0);
00014 DigitalIn user(USER_BUTTON);
00015 DigitalOut led(LED1);
00016 Timer moter;
00017 Timer turn;
00018 Timer Return;
00019 Timer screw;
00020 Timer slider;
00021 Timer _start;
00022 //lpf _distance(1.0/60, 0.05);
00023 
00024 ikarashiMDC md[] = {
00025     ikarashiMDC(2,0,SM,&serial),
00026     ikarashiMDC(2,1,SM,&serial),
00027     ikarashiMDC(2,2,SM,&serial)
00028 };
00029 
00030 int main()
00031 {
00032     double turntime,turnspeed,slidespeed,screwspeed;
00033     bool closeFlag = 0, loosenFlag = 0, leaveFlag = 0,close2Flag = 0, tightenFlag = 0, turnFlag = 0, leave2Flag = 0;
00034     int a = 0,b = 0,c = 0;
00035     float lpf_dist;
00036     led = false;
00037     Estop = true;
00038     user.mode(PullUp);
00039 //    usensor.start();
00040     wait(0.1);
00041 //    ir1.startAveraging(255);
00042     for(int i=0; i < 2; i++) md[i].braking = true;
00043     while(1)
00044     {
00045 //        float dis = ir1.getDistance();
00046 //        float ave_dis = ir1.get_Averagingdistance();
00047         for(int i = 0; i < 2; i++) md[i].setSpeed(0);
00048 //        if(ave_dis > 20.0) 
00049 //        {
00050 //            _start.start();
00051 //        }
00052 //        else
00053 //        {
00054 //            _start.stop();
00055 //            _start.reset();
00056 //        }
00057         if(user == 0) break;
00058 //        if(_start > 0.1) break;
00059     }
00060     led = true;
00061 //    moter.start();
00062     turnspeed = 0;
00063 //    closeFlag = 1;
00064     loosenFlag = 1;
00065 //    while(1)
00066 //    {
00067 //        a = 1 - user;
00068 //        c = b - a;
00069 //        b = a;
00070 //        pc.printf("%d %d %d %f %f\r\n",a,b,c,turn.read(),Return.read());
00071 //        md[0].setSpeed(sin(moter)/2);
00072 //        
00073 //        if(c == -1) 
00074 //        {
00075 //            turnspeed= 0.1;
00076 //            turn.start();
00077 //        }
00078 //        if(c == 1)
00079 //        {
00080 //            turnspeed = -0.1;
00081 //            turntime = turn.read();
00082 //            turn.stop();
00083 //            turn.reset();
00084 //            Return.start();
00085 //        }
00086 //        if(Return)
00087 //        {
00088 //            
00089 //            if(Return.read() >= turntime)
00090 //            {
00091 //                Return.stop();
00092 //                Return.reset();
00093 //                turnspeed = 0;
00094 //            }
00095 //        }
00096 //        md[1].setSpeed(turnspeed);
00097 //    }
00098 //    closeFlag = 1;
00099     while(1)
00100     {
00101 //        usensor.start();
00102 //        wait_ms(100); 
00103 //        unsigned int dist=usensor.get_dist_cm();
00104         slidespeed = 0;
00105         screwspeed = 0;
00106         turnspeed = 0;
00107 //        lpf_dist = _distance.path_value(dist);
00108         if(closeFlag)
00109         {
00110             slider.start();
00111             slidespeed = 0.15;
00112             screwspeed = 0.2;
00113 //            pc.printf("b");
00114 //            if(closeStopTrig <= slider)
00115 //            {
00116 ////                pc.printf("a");
00117 //                closeFlag = 0;
00118 //                loosenFlag = 1;
00119 //                slider.stop();
00120 //                slider.reset();
00121 //            }
00122         }
00123         if(loosenFlag)
00124         {
00125             screw.start();
00126             screwspeed = 0.7;
00127 //            slidespeed = -0.08;
00128 //            pc.printf("a");
00129             if(loosenTrig < screw)
00130             {
00131 //                pc.printf("aa");
00132                 loosenFlag = 0;
00133                 leaveFlag = 1;
00134                 screw.stop();
00135                 screw.reset();
00136             }
00137         }
00138         if(leaveFlag)
00139         {
00140             slidespeed = -0.2;
00141             slider.start();
00142 //            pc.printf("b");
00143             if(leaveStopTrig <= slider)
00144             {
00145 //                pc.printf("bb");
00146                 leaveFlag = 0;
00147                 close2Flag = 1;
00148 //                turnFlag = 1;
00149                 slider.stop();
00150                 slider.reset();
00151             }
00152         }
00153         if(turnFlag)
00154         {
00155             turnspeed = 0.8;
00156             turn.start();
00157             if(turnLimit)
00158 //            if(turnTrig < turn)
00159             {
00160                 turnFlag = 0;
00161                 close2Flag = 1;
00162                 turn.stop();
00163                 turn.reset();
00164             }
00165         }
00166         if(close2Flag)
00167         {
00168 //            turnspeed = 0.2;
00169             slidespeed = 0.2;
00170             screwspeed = 0.4;
00171             slider.start();
00172             if(closeStopTrig < slider)
00173             {
00174                 close2Flag = 0;
00175                 tightenFlag = 1;
00176                 slider.stop();
00177                 slider.reset();
00178             }
00179         }
00180         if(tightenFlag)
00181         {
00182             screwspeed = 0.4;
00183             screw.start();
00184             if(tightenTrig < screw)
00185             {
00186                 tightenFlag = 0;
00187                 leave2Flag = 1;
00188                 screw.stop();
00189                 screw.reset();
00190             }
00191         }
00192         if(leave2Flag)
00193         {
00194             slidespeed = -0.2;
00195             slider.start();
00196 //            pc.printf("b");
00197             if(leaveStopTrig <= slider)
00198             {
00199 //                pc.printf("bb");
00200                 leave2Flag = 0;
00201 //                turnFlag = 1;
00202                 slider.stop();
00203                 slider.reset();
00204             }
00205         }
00206         md[0].setSpeed(slidespeed);
00207         md[1].setSpeed(turnspeed);
00208         md[2].setSpeed(screwspeed);
00209 //        pc.printf("%f | %d\n\r", lpf_dist, dist);
00210     }
00211 }