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 ikarashiMDC_2byte_ver lpf
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 }
Generated on Sat Jul 23 2022 14:34:04 by
