ikarashiMDCslaveの改良(?)版 スピード制御の分解能が2byte
Dependencies: mbed SoftPWM MotorSMLAP
main.cpp
- Committer:
- WAT34
- Date:
- 2017-11-11
- Revision:
- 8:fd749c243a8f
- Parent:
- 6:eccd0b81ba62
- Child:
- 9:1579e5cfa499
File content as of revision 8:fd749c243a8f:
#include "mbed.h" #include "motorsmlap.h" #include "MDC3_0pinConfig.h" #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); DigitalOut RSControl(RS485_CS); DigitalOut addrChecked(LED_0); DigitalOut headerRecieved(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 = 2; DigitalOut beep(BUZER); bool brakeing[4] = {false}; int mode[4] = {0}; void forceStop() { for(int i= 0; i< 4; i++) motor[i].setMotorSpeed(0); 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) { 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{ headerRecieved = false; addrChecked = false; } 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() { 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++) { beep = true; wait(0.1); beep = false; wait(0.1); } // beep addr times addrChecked=false,headerRecieved=false; //motor[0].setMotorSpeed(0); rs485.baud(38400); serial.baud(115200); addr.mode(PullUp); RSControl = 0; rs485.putc((1<<addr)); rs485.attach(&callback); beep = false; while(1) { debugLED = addr; //serial.printf("addr: %d",addr); if(estop) forceStop(); beep = estop; wait(0.1); } }