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 SoftPWM MotorSMLAP
Diff: main.cpp
- Revision:
- 9:1579e5cfa499
- Parent:
- 8:fd749c243a8f
- Parent:
- 7:5ebe1058ca5b
- Child:
- 10:5f932239b8a5
--- a/main.cpp Sat Nov 11 00:43:54 2017 +0000 +++ b/main.cpp Sat Nov 11 00:52:25 2017 +0000 @@ -1,9 +1,10 @@ #include "mbed.h" #include "motorsmlap.h" #include "MDC3_0pinConfig.h" +#include <vector> #define TIMEOUT 0.5 using namespace pinConfig; -motorSmLap motor[]={ +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), @@ -13,8 +14,10 @@ Serial serial(UART1_TX,UART1_RX); DigitalOut RSControl(RS485_CS); -DigitalOut addrChecked(LED_0); -DigitalOut headerRecieved(LED_1); +bool addrChecked; +bool headerRecieved; +DigitalOut debugLED0(LED_0); +DigitalOut debugLED1(LED_1); BusOut debugLED(LED_2,LED_3); Timeout timeout; uint8_t pointedMotor; @@ -26,6 +29,9 @@ int mode[4] = {0}; +std::vector<unsigned char> buf; + + void forceStop() { for(int i= 0; i< 4; i++) @@ -33,81 +39,54 @@ estop = true; } -void callback(){ - const uint8_t data = rs485.getc(); - serial.putc(data); - if(data == 255) //header - { - if(headerRecieved){ - doubleHeader =true; - return; - } - else if (doubleHeader) - { - estop = true; - return; - } - addrChecked = false; - headerRecieved = true; - return; - }else - if(headerRecieved && !addrChecked) - { - //serial.printf("data :%d",data); - doubleHeader=false; - if((data>>5) == addr) - { +void callback() +{ + debugLED0 != debugLED0; + 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%4; - mode[pointedMotor] = ((data>>4)%2); - motor[pointedMotor].braking = (data>>3)%2; - //motor[pointedMotor].setMode(false); - //RSControl = 1; - //rs485.putc(addr); - //RSControl = 0; - }else{ + 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; - addrChecked = false; + serial.putc(pointedMotor); + timeout.attach(&forceStop,TIMEOUT); } - return; - }else if(headerRecieved && addrChecked) - { - //serial.printf("data %d\n\r",data); - motor[pointedMotor].setMode(mode[pointedMotor]); - doubleHeader = false; - estop = false; - if(data == 126){ - motor[pointedMotor].setMotorSpeed(0); - //serial.printf("STOP"); - }else{ - motor[pointedMotor].setMotorSpeed((data-126.0)/126.0); - } - addrChecked = false; - headerRecieved = false; - serial.putc(pointedMotor); - timeout.attach(&forceStop,TIMEOUT); - }else{ - doubleHeader = false; - headerRecieved =false; - addrChecked = false; } } -int main() { + + +int main() +{ for(int i= 0; i< 4; i++) motor[i].setMotorSpeed(0); beep = true; wait(1); beep = false; wait(0.5); - for(int i = 0; i<=addr;i++) - { + for(int i = 0; i<=addr; i++) { beep = true; wait(0.1); beep = false; wait(0.1); } // beep addr times - + + unsigned char tmp[3] = {0}; addrChecked=false,headerRecieved=false; //motor[0].setMotorSpeed(0); rs485.baud(38400); @@ -118,9 +97,18 @@ rs485.attach(&callback); beep = false; while(1) { - debugLED = addr; - - //serial.printf("addr: %d",addr); + 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); + } else { + buf.erase(buf.begin()); + } + } if(estop) forceStop(); beep = estop;