Diff: main.cpp

Revision:

0:73bc775f95cf

diff -r 000000000000 -r 73bc775f95cf main.cpp
--- /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