NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2020BAuto

NHK2020 B tukemonoisi

Dependencies:   mbed ikarashiMDC_2byte_ver lpf

main.cpp@0:73bc775f95cf, 2020-11-07 (annotated)

Committer:
THtakahiro702286
Date:
Sat Nov 07 04:41:53 2020 +0000
Revision:
0:73bc775f95cf
2020BAuto

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:73bc775f95cf 1 #include "mbed.h"
THtakahiro702286 0:73bc775f95cf 2 //#include"IRsensor.h"
THtakahiro702286 0:73bc775f95cf 3 #include "ikarashiMDC.h"
THtakahiro702286 0:73bc775f95cf 4 //#include "hcsr04.h"
THtakahiro702286 0:73bc775f95cf 5 #include "pin_config.h"
THtakahiro702286 0:73bc775f95cf 6 #include "lpf.h"
THtakahiro702286 0:73bc775f95cf 7
THtakahiro702286 0:73bc775f95cf 8 Serial pc(USBTX,USBRX,115200);
THtakahiro702286 0:73bc775f95cf 9 DigitalOut Estop(D12);
THtakahiro702286 0:73bc775f95cf 10 Serial serial(PC_10, PC_11, 115200);
THtakahiro702286 0:73bc775f95cf 11 DigitalIn turnLimit(PB_3);
THtakahiro702286 0:73bc775f95cf 12 //HCSR04 usensor(D9,D10);
THtakahiro702286 0:73bc775f95cf 13 //IRsensor ir1(PC_0);
THtakahiro702286 0:73bc775f95cf 14 DigitalIn user(USER_BUTTON);
THtakahiro702286 0:73bc775f95cf 15 DigitalOut led(LED1);
THtakahiro702286 0:73bc775f95cf 16 Timer moter;
THtakahiro702286 0:73bc775f95cf 17 Timer turn;
THtakahiro702286 0:73bc775f95cf 18 Timer Return;
THtakahiro702286 0:73bc775f95cf 19 Timer screw;
THtakahiro702286 0:73bc775f95cf 20 Timer slider;
THtakahiro702286 0:73bc775f95cf 21 Timer _start;
THtakahiro702286 0:73bc775f95cf 22 //lpf _distance(1.0/60, 0.05);
THtakahiro702286 0:73bc775f95cf 23
THtakahiro702286 0:73bc775f95cf 24 ikarashiMDC md[] = {
THtakahiro702286 0:73bc775f95cf 25 ikarashiMDC(2,0,SM,&serial),
THtakahiro702286 0:73bc775f95cf 26 ikarashiMDC(2,1,SM,&serial),
THtakahiro702286 0:73bc775f95cf 27 ikarashiMDC(2,2,SM,&serial)
THtakahiro702286 0:73bc775f95cf 28 };
THtakahiro702286 0:73bc775f95cf 29
THtakahiro702286 0:73bc775f95cf 30 int main()
THtakahiro702286 0:73bc775f95cf 31 {
THtakahiro702286 0:73bc775f95cf 32 double turntime,turnspeed,slidespeed,screwspeed;
THtakahiro702286 0:73bc775f95cf 33 bool closeFlag = 0, loosenFlag = 0, leaveFlag = 0,close2Flag = 0, tightenFlag = 0, turnFlag = 0, leave2Flag = 0;
THtakahiro702286 0:73bc775f95cf 34 int a = 0,b = 0,c = 0;
THtakahiro702286 0:73bc775f95cf 35 float lpf_dist;
THtakahiro702286 0:73bc775f95cf 36 led = false;
THtakahiro702286 0:73bc775f95cf 37 Estop = true;
THtakahiro702286 0:73bc775f95cf 38 user.mode(PullUp);
THtakahiro702286 0:73bc775f95cf 39 // usensor.start();
THtakahiro702286 0:73bc775f95cf 40 wait(0.1);
THtakahiro702286 0:73bc775f95cf 41 // ir1.startAveraging(255);
THtakahiro702286 0:73bc775f95cf 42 for(int i=0; i < 2; i++) md[i].braking = true;
THtakahiro702286 0:73bc775f95cf 43 while(1)
THtakahiro702286 0:73bc775f95cf 44 {
THtakahiro702286 0:73bc775f95cf 45 // float dis = ir1.getDistance();
THtakahiro702286 0:73bc775f95cf 46 // float ave_dis = ir1.get_Averagingdistance();
THtakahiro702286 0:73bc775f95cf 47 for(int i = 0; i < 2; i++) md[i].setSpeed(0);
THtakahiro702286 0:73bc775f95cf 48 // if(ave_dis > 20.0)
THtakahiro702286 0:73bc775f95cf 49 // {
THtakahiro702286 0:73bc775f95cf 50 // _start.start();
THtakahiro702286 0:73bc775f95cf 51 // }
THtakahiro702286 0:73bc775f95cf 52 // else
THtakahiro702286 0:73bc775f95cf 53 // {
THtakahiro702286 0:73bc775f95cf 54 // _start.stop();
THtakahiro702286 0:73bc775f95cf 55 // _start.reset();
THtakahiro702286 0:73bc775f95cf 56 // }
THtakahiro702286 0:73bc775f95cf 57 if(user == 0) break;
THtakahiro702286 0:73bc775f95cf 58 // if(_start > 0.1) break;
THtakahiro702286 0:73bc775f95cf 59 }
THtakahiro702286 0:73bc775f95cf 60 led = true;
THtakahiro702286 0:73bc775f95cf 61 // moter.start();
THtakahiro702286 0:73bc775f95cf 62 turnspeed = 0;
THtakahiro702286 0:73bc775f95cf 63 // closeFlag = 1;
THtakahiro702286 0:73bc775f95cf 64 loosenFlag = 1;
THtakahiro702286 0:73bc775f95cf 65 // while(1)
THtakahiro702286 0:73bc775f95cf 66 // {
THtakahiro702286 0:73bc775f95cf 67 // a = 1 - user;
THtakahiro702286 0:73bc775f95cf 68 // c = b - a;
THtakahiro702286 0:73bc775f95cf 69 // b = a;
THtakahiro702286 0:73bc775f95cf 70 // pc.printf("%d %d %d %f %f\r\n",a,b,c,turn.read(),Return.read());
THtakahiro702286 0:73bc775f95cf 71 // md[0].setSpeed(sin(moter)/2);
THtakahiro702286 0:73bc775f95cf 72 //
THtakahiro702286 0:73bc775f95cf 73 // if(c == -1)
THtakahiro702286 0:73bc775f95cf 74 // {
THtakahiro702286 0:73bc775f95cf 75 // turnspeed= 0.1;
THtakahiro702286 0:73bc775f95cf 76 // turn.start();
THtakahiro702286 0:73bc775f95cf 77 // }
THtakahiro702286 0:73bc775f95cf 78 // if(c == 1)
THtakahiro702286 0:73bc775f95cf 79 // {
THtakahiro702286 0:73bc775f95cf 80 // turnspeed = -0.1;
THtakahiro702286 0:73bc775f95cf 81 // turntime = turn.read();
THtakahiro702286 0:73bc775f95cf 82 // turn.stop();
THtakahiro702286 0:73bc775f95cf 83 // turn.reset();
THtakahiro702286 0:73bc775f95cf 84 // Return.start();
THtakahiro702286 0:73bc775f95cf 85 // }
THtakahiro702286 0:73bc775f95cf 86 // if(Return)
THtakahiro702286 0:73bc775f95cf 87 // {
THtakahiro702286 0:73bc775f95cf 88 //
THtakahiro702286 0:73bc775f95cf 89 // if(Return.read() >= turntime)
THtakahiro702286 0:73bc775f95cf 90 // {
THtakahiro702286 0:73bc775f95cf 91 // Return.stop();
THtakahiro702286 0:73bc775f95cf 92 // Return.reset();
THtakahiro702286 0:73bc775f95cf 93 // turnspeed = 0;
THtakahiro702286 0:73bc775f95cf 94 // }
THtakahiro702286 0:73bc775f95cf 95 // }
THtakahiro702286 0:73bc775f95cf 96 // md[1].setSpeed(turnspeed);
THtakahiro702286 0:73bc775f95cf 97 // }
THtakahiro702286 0:73bc775f95cf 98 // closeFlag = 1;
THtakahiro702286 0:73bc775f95cf 99 while(1)
THtakahiro702286 0:73bc775f95cf 100 {
THtakahiro702286 0:73bc775f95cf 101 // usensor.start();
THtakahiro702286 0:73bc775f95cf 102 // wait_ms(100);
THtakahiro702286 0:73bc775f95cf 103 // unsigned int dist=usensor.get_dist_cm();
THtakahiro702286 0:73bc775f95cf 104 slidespeed = 0;
THtakahiro702286 0:73bc775f95cf 105 screwspeed = 0;
THtakahiro702286 0:73bc775f95cf 106 turnspeed = 0;
THtakahiro702286 0:73bc775f95cf 107 // lpf_dist = _distance.path_value(dist);
THtakahiro702286 0:73bc775f95cf 108 if(closeFlag)
THtakahiro702286 0:73bc775f95cf 109 {
THtakahiro702286 0:73bc775f95cf 110 slider.start();
THtakahiro702286 0:73bc775f95cf 111 slidespeed = 0.15;
THtakahiro702286 0:73bc775f95cf 112 screwspeed = 0.2;
THtakahiro702286 0:73bc775f95cf 113 // pc.printf("b");
THtakahiro702286 0:73bc775f95cf 114 // if(closeStopTrig <= slider)
THtakahiro702286 0:73bc775f95cf 115 // {
THtakahiro702286 0:73bc775f95cf 116 //// pc.printf("a");
THtakahiro702286 0:73bc775f95cf 117 // closeFlag = 0;
THtakahiro702286 0:73bc775f95cf 118 // loosenFlag = 1;
THtakahiro702286 0:73bc775f95cf 119 // slider.stop();
THtakahiro702286 0:73bc775f95cf 120 // slider.reset();
THtakahiro702286 0:73bc775f95cf 121 // }
THtakahiro702286 0:73bc775f95cf 122 }
THtakahiro702286 0:73bc775f95cf 123 if(loosenFlag)
THtakahiro702286 0:73bc775f95cf 124 {
THtakahiro702286 0:73bc775f95cf 125 screw.start();
THtakahiro702286 0:73bc775f95cf 126 screwspeed = 0.7;
THtakahiro702286 0:73bc775f95cf 127 // slidespeed = -0.08;
THtakahiro702286 0:73bc775f95cf 128 // pc.printf("a");
THtakahiro702286 0:73bc775f95cf 129 if(loosenTrig < screw)
THtakahiro702286 0:73bc775f95cf 130 {
THtakahiro702286 0:73bc775f95cf 131 // pc.printf("aa");
THtakahiro702286 0:73bc775f95cf 132 loosenFlag = 0;
THtakahiro702286 0:73bc775f95cf 133 leaveFlag = 1;
THtakahiro702286 0:73bc775f95cf 134 screw.stop();
THtakahiro702286 0:73bc775f95cf 135 screw.reset();
THtakahiro702286 0:73bc775f95cf 136 }
THtakahiro702286 0:73bc775f95cf 137 }
THtakahiro702286 0:73bc775f95cf 138 if(leaveFlag)
THtakahiro702286 0:73bc775f95cf 139 {
THtakahiro702286 0:73bc775f95cf 140 slidespeed = -0.2;
THtakahiro702286 0:73bc775f95cf 141 slider.start();
THtakahiro702286 0:73bc775f95cf 142 // pc.printf("b");
THtakahiro702286 0:73bc775f95cf 143 if(leaveStopTrig <= slider)
THtakahiro702286 0:73bc775f95cf 144 {
THtakahiro702286 0:73bc775f95cf 145 // pc.printf("bb");
THtakahiro702286 0:73bc775f95cf 146 leaveFlag = 0;
THtakahiro702286 0:73bc775f95cf 147 close2Flag = 1;
THtakahiro702286 0:73bc775f95cf 148 // turnFlag = 1;
THtakahiro702286 0:73bc775f95cf 149 slider.stop();
THtakahiro702286 0:73bc775f95cf 150 slider.reset();
THtakahiro702286 0:73bc775f95cf 151 }
THtakahiro702286 0:73bc775f95cf 152 }
THtakahiro702286 0:73bc775f95cf 153 if(turnFlag)
THtakahiro702286 0:73bc775f95cf 154 {
THtakahiro702286 0:73bc775f95cf 155 turnspeed = 0.8;
THtakahiro702286 0:73bc775f95cf 156 turn.start();
THtakahiro702286 0:73bc775f95cf 157 if(turnLimit)
THtakahiro702286 0:73bc775f95cf 158 // if(turnTrig < turn)
THtakahiro702286 0:73bc775f95cf 159 {
THtakahiro702286 0:73bc775f95cf 160 turnFlag = 0;
THtakahiro702286 0:73bc775f95cf 161 close2Flag = 1;
THtakahiro702286 0:73bc775f95cf 162 turn.stop();
THtakahiro702286 0:73bc775f95cf 163 turn.reset();
THtakahiro702286 0:73bc775f95cf 164 }
THtakahiro702286 0:73bc775f95cf 165 }
THtakahiro702286 0:73bc775f95cf 166 if(close2Flag)
THtakahiro702286 0:73bc775f95cf 167 {
THtakahiro702286 0:73bc775f95cf 168 // turnspeed = 0.2;
THtakahiro702286 0:73bc775f95cf 169 slidespeed = 0.2;
THtakahiro702286 0:73bc775f95cf 170 screwspeed = 0.4;
THtakahiro702286 0:73bc775f95cf 171 slider.start();
THtakahiro702286 0:73bc775f95cf 172 if(closeStopTrig < slider)
THtakahiro702286 0:73bc775f95cf 173 {
THtakahiro702286 0:73bc775f95cf 174 close2Flag = 0;
THtakahiro702286 0:73bc775f95cf 175 tightenFlag = 1;
THtakahiro702286 0:73bc775f95cf 176 slider.stop();
THtakahiro702286 0:73bc775f95cf 177 slider.reset();
THtakahiro702286 0:73bc775f95cf 178 }
THtakahiro702286 0:73bc775f95cf 179 }
THtakahiro702286 0:73bc775f95cf 180 if(tightenFlag)
THtakahiro702286 0:73bc775f95cf 181 {
THtakahiro702286 0:73bc775f95cf 182 screwspeed = 0.4;
THtakahiro702286 0:73bc775f95cf 183 screw.start();
THtakahiro702286 0:73bc775f95cf 184 if(tightenTrig < screw)
THtakahiro702286 0:73bc775f95cf 185 {
THtakahiro702286 0:73bc775f95cf 186 tightenFlag = 0;
THtakahiro702286 0:73bc775f95cf 187 leave2Flag = 1;
THtakahiro702286 0:73bc775f95cf 188 screw.stop();
THtakahiro702286 0:73bc775f95cf 189 screw.reset();
THtakahiro702286 0:73bc775f95cf 190 }
THtakahiro702286 0:73bc775f95cf 191 }
THtakahiro702286 0:73bc775f95cf 192 if(leave2Flag)
THtakahiro702286 0:73bc775f95cf 193 {
THtakahiro702286 0:73bc775f95cf 194 slidespeed = -0.2;
THtakahiro702286 0:73bc775f95cf 195 slider.start();
THtakahiro702286 0:73bc775f95cf 196 // pc.printf("b");
THtakahiro702286 0:73bc775f95cf 197 if(leaveStopTrig <= slider)
THtakahiro702286 0:73bc775f95cf 198 {
THtakahiro702286 0:73bc775f95cf 199 // pc.printf("bb");
THtakahiro702286 0:73bc775f95cf 200 leave2Flag = 0;
THtakahiro702286 0:73bc775f95cf 201 // turnFlag = 1;
THtakahiro702286 0:73bc775f95cf 202 slider.stop();
THtakahiro702286 0:73bc775f95cf 203 slider.reset();
THtakahiro702286 0:73bc775f95cf 204 }
THtakahiro702286 0:73bc775f95cf 205 }
THtakahiro702286 0:73bc775f95cf 206 md[0].setSpeed(slidespeed);
THtakahiro702286 0:73bc775f95cf 207 md[1].setSpeed(turnspeed);
THtakahiro702286 0:73bc775f95cf 208 md[2].setSpeed(screwspeed);
THtakahiro702286 0:73bc775f95cf 209 // pc.printf("%f | %d\n\r", lpf_dist, dist);
THtakahiro702286 0:73bc775f95cf 210 }
THtakahiro702286 0:73bc775f95cf 211 }