Revision 33:ef0d51f3aa23, committed 2018-12-02

Comitter:

tknara

Date:

Sun Dec 02 03:23:35 2018 +0000

Parent:

32:c64104c77531

Child:

34:a00d3e06d81f

Commit message:

filter worked;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorSMLAP.lib	Sun Dec 02 03:23:35 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/MotorSMLAP/#5995b899cdcc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SoftPWM.lib	Sun Dec 02 03:23:35 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/komaida424/code/SoftPWM/#7918ce37626c

--- a/main.cpp	Sat Dec 01 06:41:51 2018 +0000
+++ b/main.cpp	Sun Dec 02 03:23:35 2018 +0000
@@ -1,28 +1,152 @@
 #include "mbed.h"
+#include "motorsmlap.h"
 #include "MDC3_0pinConfig.h"
+#include "SoftPWM.h"
+#include <vector>
+#define TIMEOUT 0.5
 using namespace pinConfig;
+motorSmLap motor[]= {
+    motorSmLap(DIR_H_0,DIR_L_0,PWM0),
+    motorSmLap(DIR_H_1,DIR_L_1,PWM1),
+    motorSmLap(DIR_H_2,DIR_L_2,PWM2),
+    motorSmLap(DIR_H_3,DIR_L_3,PWM3)
+};
+Serial rs485(RS485_TX,RS485_RX);
+Serial serial(UART1_TX,UART1_RX);
 CAN can(CAN_RX, CAN_TX);
-Serial pc(PA_9,PA_10,115200);
+
+DigitalOut RSControl(RS485_CS);
+bool addrChecked;
+bool headerRecieved;
 DigitalOut debugLED0(LED_0);
-bool canRecieved=false;
+DigitalOut debugLED1(LED_1);
+BusOut debugLED(LED_2,LED_3);
+Timeout timeout;
+uint8_t pointedMotor;
+bool estop =false,doubleHeader = false;
+//BusIn addr(DIP_0,DIP_1,DIP_2);
+uint8_t addr = 3;
+SoftPWM beep(BUZER);
+bool brakeing[4] = {false};
+bool canRecieved = false;
+
+int mode[4] = {0};
+
+std::vector<unsigned char> buf;
+uint8_t canbuf[8];
 CANMessage msg;
+
 void canRxIt()
 {
-    if(can.read(msg))//&&(msg.id==addr)&&(msg.len==4))
+    if(can.read(msg))
     {
         canRecieved = true;
+        estop = false;
+    }
+}
+
+void forceStop()
+{
+    for(int i= 0; i< 4; i++)
+        motor[i].setMotorSpeed(0);
+    estop = true;
+}
+
+void callback()
+{
+    buf.push_back(rs485.getc());
+}
+
+void processData(unsigned char data[])
+{
+    if(data[0]^data[1] == data[2]) {
+        if((data[0]>>5) == addr) {
+            addrChecked =true;
+            pointedMotor = data[0]%4;
+            mode[pointedMotor] = ((data[0]>>4)%2);
+            motor[pointedMotor].braking = (data[0]>>3)%2;
+
+            motor[pointedMotor].setMode(mode[pointedMotor]);
+            if(data[1] == 126) {
+                motor[pointedMotor].setMotorSpeed(0);
+                //serial.printf("STOP");
+            } else {
+                motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
+            }
+            addrChecked = false;
+            headerRecieved = false;
+            serial.putc(pointedMotor);
+            timeout.attach(&forceStop,TIMEOUT);
+        }
     }
 }
-int main() {
-    pc.printf("main()\n");
-    can.frequency(500000);
+int main()
+{
+    for(int i= 0; i< 4; i++)
+        motor[i].setMotorSpeed(0);
+    beep.period(1.0 / 2000.0);
+    beep = 0.6;
+    wait(0.1);
+    beep.period(1.0 / 4000.0);
+    beep = 0.6;
+    wait(0.1);
+    beep.period(1.0 / 6000.0);
+    beep = 0.6;
+    wait(0.1);
+    beep = 0.0;
+    wait(0.1);
+    beep.period(1.0 / 2000.0);
+    for(int i = 0; i<addr; i++) {
+        beep = 0.6;
+        wait(0.075);
+        beep = 0.0;
+        wait(0.075);
+    } // beep addr times
+    unsigned char tmp[3] = {0};
+    addrChecked=false,headerRecieved=false;
+    rs485.baud(115200);
+    serial.baud(921600);
+    //addr.mode(PullUp);
+    RSControl = 0;
+    rs485.putc((1<<addr));
+    rs485.attach(&callback);
+    beep = 0.0;
+    can.frequency(125000);
+    can.filter(addr,0x7ff,CANStandard);
+    serial.printf("addr:%d\n",addr);
+    wait(0.01); 
     can.attach(&canRxIt, CAN::RxIrq);
     while(1) {
+        debugLED = addr;
+        if(buf.size() > 4) {
+            if (buf[0] == 255) {
+                tmp[0] = buf[1];
+                tmp[1] = buf[2];
+                tmp[2] = buf[3];
+                processData(tmp);
+                buf.erase(buf.begin(),buf.begin()+3);
+                debugLED0 = !debugLED0;
+            } else {
+                buf.erase(buf.begin());
+                debugLED0 = false;
+            }
+        }
         if(canRecieved)
         {
+            for(int i=0;i<4;i++)
+            {   
+                serial.printf("num:%d,data:%d",i,msg.data[i]);
+                motor[i].setMotorSpeed((msg.data[i]-126.0)/126.0); 
+            }
+            serial.printf("\n");
+            //processData(tmp);
             debugLED0 = !debugLED0;
             canRecieved = false;
         }
+        if(estop)
+            forceStop();
+        beep = estop*0.5;
+        //wait(0.001);
     }
 }