ikarashiMDCslaveの改良(?)版 スピード制御の分解能が2byte
Dependencies: mbed SoftPWM MotorSMLAP
main.cpp
- Committer:
- skouki
- Date:
- 2019-09-02
- Revision:
- 16:8c2eb2b4bead
- Parent:
- 15:0db5eea67306
- Child:
- 17:1202d7305eab
File content as of revision 16:8c2eb2b4bead:
#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); DigitalOut RSControl(RS485_CS); bool addrChecked; bool headerRecieved; DigitalIn test1(pinConfig::I2C_SDA); DigitalIn test2(pinConfig::I2C_SCL); DigitalOut debugLED0(LED_0); 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 = 2; SoftPWM beep(BUZER); bool brakeing[4] = {false}; bool canRecieved = false; int mode[4] = {0}; std::vector<unsigned char> buf; uint8_t canbuf[4]; CANMessage msg; bool testing = false; void start(); void runTest() { testing = true; //beep = 1; for(int i = 0;i < 4;i++) { motor[i].setFreq(1600*i); motor[i].setMotorSpeed(0.1); wait(0.3); motor[i].setMotorSpeed(0); motor[i].setFreq(20000.0); } testing = false; //beep = 0; } void canRxIt() { if(can.read(msg)&&(msg.id==addr)) { canbuf[0] = msg.data[0]; canbuf[1] = msg.data[1]; canbuf[2] = msg.data[2]; canbuf[3] = msg.data[3]; canRecieved = true; } } void forceStop() { if(testing) return; 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] == data[3]) { 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] + (data[2]<<8)) == 32768) { motor[pointedMotor].setMotorSpeed(0); //serial.printf("STOP"); } else { motor[pointedMotor].setMotorSpeed(((data[1] + (data[2]<<8))-32768.0)/32768.0); } addrChecked = false; headerRecieved = false; serial.putc(pointedMotor); timeout.attach(&forceStop,TIMEOUT); } } } void start() { for(int i= 0; i< 4; i++) { motor[i].setMode(SM); motor[i].setMotorSpeed(0); } beep.period(1.0 / 2959.955); beep = 0.2; wait(0.4); beep = 0.0; wait(0.1); beep.period(1.0 / 2637.020); beep = 0.2; wait(0.4); beep = 0.0; wait(0.1); beep.period(1.0 / 3322.438); for(int i = 0; i<addr; i++) { beep = 0.2; wait(0.4); beep = 0.0; wait(0.075); } // beep addr times beep.period(1.0 / 1000.0); } int main() { start(); unsigned char tmp[4] = {0}; addrChecked=false,headerRecieved=false; can.attach(&canRxIt, CAN::RxIrq); //motor[0].setMotorSpeed(0); rs485.baud(115200); serial.baud(115200); addr.mode(PullUp); test1.mode(PullDown); test2.mode(PullDown); RSControl = 0; rs485.putc((1<<addr)); rs485.attach(&callback); beep = 0.0; while(1) { debugLED = addr; if(buf.size() > 5) { if (buf[0] == 255) { tmp[0] = buf[1]; tmp[1] = buf[2]; tmp[2] = buf[3]; tmp[3] = buf[4]; processData(tmp); buf.erase(buf.begin(),buf.begin()+4); debugLED0 = !debugLED0; } else { buf.erase(buf.begin()); debugLED0 = false; } } if(canRecieved) { tmp[0] = canbuf[0]; tmp[1] = canbuf[1]; tmp[2] = canbuf[2]; tmp[3] = canbuf[3]; processData(tmp); debugLED0 = !debugLED0; canRecieved = false; } if(estop) forceStop(); beep = estop; if(test1 && test2) runTest(); } }