NagaokaRoboticsClub_mbedTeam
NHK2020 B tukemonoisi
Dependencies: mbed ikarashiMDC_2byte_ver lpf
Revision 0:73bc775f95cf, committed 2020-11-07
- Comitter:
- THtakahiro702286
- Date:
- Sat Nov 07 04:41:53 2020 +0000
- Commit message:
- 2020BAuto
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ikarashiMDC_2byte_ver.lib Sat Nov 07 04:41:53 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC_2byte_ver/#97bb662f1e1f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lpf.lib Sat Nov 07 04:41:53 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/THtakahiro702286/code/lpf/#dedf16377e0f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Nov 07 04:41:53 2020 +0000
@@ -0,0 +1,211 @@
+#include "mbed.h"
+//#include"IRsensor.h"
+#include "ikarashiMDC.h"
+//#include "hcsr04.h"
+#include "pin_config.h"
+#include "lpf.h"
+
+Serial pc(USBTX,USBRX,115200);
+DigitalOut Estop(D12);
+Serial serial(PC_10, PC_11, 115200);
+DigitalIn turnLimit(PB_3);
+//HCSR04 usensor(D9,D10);
+//IRsensor ir1(PC_0);
+DigitalIn user(USER_BUTTON);
+DigitalOut led(LED1);
+Timer moter;
+Timer turn;
+Timer Return;
+Timer screw;
+Timer slider;
+Timer _start;
+//lpf _distance(1.0/60, 0.05);
+
+ikarashiMDC md[] = {
+ ikarashiMDC(2,0,SM,&serial),
+ ikarashiMDC(2,1,SM,&serial),
+ ikarashiMDC(2,2,SM,&serial)
+};
+
+int main()
+{
+ double turntime,turnspeed,slidespeed,screwspeed;
+ bool closeFlag = 0, loosenFlag = 0, leaveFlag = 0,close2Flag = 0, tightenFlag = 0, turnFlag = 0, leave2Flag = 0;
+ int a = 0,b = 0,c = 0;
+ float lpf_dist;
+ led = false;
+ Estop = true;
+ user.mode(PullUp);
+// usensor.start();
+ wait(0.1);
+// ir1.startAveraging(255);
+ for(int i=0; i < 2; i++) md[i].braking = true;
+ while(1)
+ {
+// float dis = ir1.getDistance();
+// float ave_dis = ir1.get_Averagingdistance();
+ for(int i = 0; i < 2; i++) md[i].setSpeed(0);
+// if(ave_dis > 20.0)
+// {
+// _start.start();
+// }
+// else
+// {
+// _start.stop();
+// _start.reset();
+// }
+ if(user == 0) break;
+// if(_start > 0.1) break;
+ }
+ led = true;
+// moter.start();
+ turnspeed = 0;
+// closeFlag = 1;
+ loosenFlag = 1;
+// while(1)
+// {
+// a = 1 - user;
+// c = b - a;
+// b = a;
+// pc.printf("%d %d %d %f %f\r\n",a,b,c,turn.read(),Return.read());
+// md[0].setSpeed(sin(moter)/2);
+//
+// if(c == -1)
+// {
+// turnspeed= 0.1;
+// turn.start();
+// }
+// if(c == 1)
+// {
+// turnspeed = -0.1;
+// turntime = turn.read();
+// turn.stop();
+// turn.reset();
+// Return.start();
+// }
+// if(Return)
+// {
+//
+// if(Return.read() >= turntime)
+// {
+// Return.stop();
+// Return.reset();
+// turnspeed = 0;
+// }
+// }
+// md[1].setSpeed(turnspeed);
+// }
+// closeFlag = 1;
+ while(1)
+ {
+// usensor.start();
+// wait_ms(100);
+// unsigned int dist=usensor.get_dist_cm();
+ slidespeed = 0;
+ screwspeed = 0;
+ turnspeed = 0;
+// lpf_dist = _distance.path_value(dist);
+ if(closeFlag)
+ {
+ slider.start();
+ slidespeed = 0.15;
+ screwspeed = 0.2;
+// pc.printf("b");
+// if(closeStopTrig <= slider)
+// {
+//// pc.printf("a");
+// closeFlag = 0;
+// loosenFlag = 1;
+// slider.stop();
+// slider.reset();
+// }
+ }
+ if(loosenFlag)
+ {
+ screw.start();
+ screwspeed = 0.7;
+// slidespeed = -0.08;
+// pc.printf("a");
+ if(loosenTrig < screw)
+ {
+// pc.printf("aa");
+ loosenFlag = 0;
+ leaveFlag = 1;
+ screw.stop();
+ screw.reset();
+ }
+ }
+ if(leaveFlag)
+ {
+ slidespeed = -0.2;
+ slider.start();
+// pc.printf("b");
+ if(leaveStopTrig <= slider)
+ {
+// pc.printf("bb");
+ leaveFlag = 0;
+ close2Flag = 1;
+// turnFlag = 1;
+ slider.stop();
+ slider.reset();
+ }
+ }
+ if(turnFlag)
+ {
+ turnspeed = 0.8;
+ turn.start();
+ if(turnLimit)
+// if(turnTrig < turn)
+ {
+ turnFlag = 0;
+ close2Flag = 1;
+ turn.stop();
+ turn.reset();
+ }
+ }
+ if(close2Flag)
+ {
+// turnspeed = 0.2;
+ slidespeed = 0.2;
+ screwspeed = 0.4;
+ slider.start();
+ if(closeStopTrig < slider)
+ {
+ close2Flag = 0;
+ tightenFlag = 1;
+ slider.stop();
+ slider.reset();
+ }
+ }
+ if(tightenFlag)
+ {
+ screwspeed = 0.4;
+ screw.start();
+ if(tightenTrig < screw)
+ {
+ tightenFlag = 0;
+ leave2Flag = 1;
+ screw.stop();
+ screw.reset();
+ }
+ }
+ if(leave2Flag)
+ {
+ slidespeed = -0.2;
+ slider.start();
+// pc.printf("b");
+ if(leaveStopTrig <= slider)
+ {
+// pc.printf("bb");
+ leave2Flag = 0;
+// turnFlag = 1;
+ slider.stop();
+ slider.reset();
+ }
+ }
+ md[0].setSpeed(slidespeed);
+ md[1].setSpeed(turnspeed);
+ md[2].setSpeed(screwspeed);
+// pc.printf("%f | %d\n\r", lpf_dist, dist);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Nov 07 04:41:53 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pin_config.h Sat Nov 07 04:41:53 2020 +0000 @@ -0,0 +1,11 @@ +#ifndef PIN_CONFIG_H +#define PIN_CONFIG_H + +const int startTrig = 1; +const float closeStopTrig = 1.0; +const float loosenTrig = 3.0; +const float leaveStopTrig = 1.0; +const float tightenTrig = 2.0; +const float turnTrig = 1.3; + +#endif \ No newline at end of file