Can test program.Maybe filter is not implemented
Dependencies: SoftPWM MotorSMLAP
Fork of CANnucleo_Hello by
main.cpp@34:a00d3e06d81f, 2018-12-04 (annotated)
- Committer:
- tknara
- Date:
- Tue Dec 04 04:21:04 2018 +0000
- Revision:
- 34:a00d3e06d81f
- Parent:
- 33:ef0d51f3aa23
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hudakz | 16:a86f339d1c25 | 1 | #include "mbed.h" |
tknara | 33:ef0d51f3aa23 | 2 | #include "motorsmlap.h" |
tknara | 32:c64104c77531 | 3 | #include "MDC3_0pinConfig.h" |
tknara | 33:ef0d51f3aa23 | 4 | #include "SoftPWM.h" |
tknara | 33:ef0d51f3aa23 | 5 | #include <vector> |
tknara | 33:ef0d51f3aa23 | 6 | #define TIMEOUT 0.5 |
tknara | 32:c64104c77531 | 7 | using namespace pinConfig; |
tknara | 33:ef0d51f3aa23 | 8 | motorSmLap motor[]= { |
tknara | 33:ef0d51f3aa23 | 9 | motorSmLap(DIR_H_0,DIR_L_0,PWM0), |
tknara | 33:ef0d51f3aa23 | 10 | motorSmLap(DIR_H_1,DIR_L_1,PWM1), |
tknara | 33:ef0d51f3aa23 | 11 | motorSmLap(DIR_H_2,DIR_L_2,PWM2), |
tknara | 33:ef0d51f3aa23 | 12 | motorSmLap(DIR_H_3,DIR_L_3,PWM3) |
tknara | 33:ef0d51f3aa23 | 13 | }; |
tknara | 33:ef0d51f3aa23 | 14 | Serial rs485(RS485_TX,RS485_RX); |
tknara | 33:ef0d51f3aa23 | 15 | Serial serial(UART1_TX,UART1_RX); |
tknara | 32:c64104c77531 | 16 | CAN can(CAN_RX, CAN_TX); |
tknara | 33:ef0d51f3aa23 | 17 | |
tknara | 33:ef0d51f3aa23 | 18 | DigitalOut RSControl(RS485_CS); |
tknara | 33:ef0d51f3aa23 | 19 | bool addrChecked; |
tknara | 33:ef0d51f3aa23 | 20 | bool headerRecieved; |
tknara | 32:c64104c77531 | 21 | DigitalOut debugLED0(LED_0); |
tknara | 33:ef0d51f3aa23 | 22 | DigitalOut debugLED1(LED_1); |
tknara | 33:ef0d51f3aa23 | 23 | BusOut debugLED(LED_2,LED_3); |
tknara | 33:ef0d51f3aa23 | 24 | Timeout timeout; |
tknara | 33:ef0d51f3aa23 | 25 | uint8_t pointedMotor; |
tknara | 33:ef0d51f3aa23 | 26 | bool estop =false,doubleHeader = false; |
tknara | 33:ef0d51f3aa23 | 27 | //BusIn addr(DIP_0,DIP_1,DIP_2); |
tknara | 33:ef0d51f3aa23 | 28 | uint8_t addr = 3; |
tknara | 33:ef0d51f3aa23 | 29 | SoftPWM beep(BUZER); |
tknara | 33:ef0d51f3aa23 | 30 | bool brakeing[4] = {false}; |
tknara | 33:ef0d51f3aa23 | 31 | bool canRecieved = false; |
tknara | 33:ef0d51f3aa23 | 32 | |
tknara | 33:ef0d51f3aa23 | 33 | int mode[4] = {0}; |
tknara | 33:ef0d51f3aa23 | 34 | |
tknara | 33:ef0d51f3aa23 | 35 | std::vector<unsigned char> buf; |
tknara | 33:ef0d51f3aa23 | 36 | uint8_t canbuf[8]; |
tknara | 32:c64104c77531 | 37 | CANMessage msg; |
tknara | 33:ef0d51f3aa23 | 38 | |
tknara | 32:c64104c77531 | 39 | void canRxIt() |
tknara | 32:c64104c77531 | 40 | { |
tknara | 33:ef0d51f3aa23 | 41 | if(can.read(msg)) |
tknara | 32:c64104c77531 | 42 | { |
tknara | 32:c64104c77531 | 43 | canRecieved = true; |
tknara | 33:ef0d51f3aa23 | 44 | estop = false; |
tknara | 33:ef0d51f3aa23 | 45 | } |
tknara | 33:ef0d51f3aa23 | 46 | } |
tknara | 33:ef0d51f3aa23 | 47 | |
tknara | 33:ef0d51f3aa23 | 48 | void forceStop() |
tknara | 33:ef0d51f3aa23 | 49 | { |
tknara | 33:ef0d51f3aa23 | 50 | for(int i= 0; i< 4; i++) |
tknara | 33:ef0d51f3aa23 | 51 | motor[i].setMotorSpeed(0); |
tknara | 33:ef0d51f3aa23 | 52 | estop = true; |
tknara | 33:ef0d51f3aa23 | 53 | } |
tknara | 33:ef0d51f3aa23 | 54 | |
tknara | 33:ef0d51f3aa23 | 55 | void callback() |
tknara | 33:ef0d51f3aa23 | 56 | { |
tknara | 33:ef0d51f3aa23 | 57 | buf.push_back(rs485.getc()); |
tknara | 33:ef0d51f3aa23 | 58 | } |
tknara | 33:ef0d51f3aa23 | 59 | |
tknara | 33:ef0d51f3aa23 | 60 | void processData(unsigned char data[]) |
tknara | 33:ef0d51f3aa23 | 61 | { |
tknara | 33:ef0d51f3aa23 | 62 | if(data[0]^data[1] == data[2]) { |
tknara | 33:ef0d51f3aa23 | 63 | if((data[0]>>5) == addr) { |
tknara | 33:ef0d51f3aa23 | 64 | addrChecked =true; |
tknara | 33:ef0d51f3aa23 | 65 | pointedMotor = data[0]%4; |
tknara | 33:ef0d51f3aa23 | 66 | mode[pointedMotor] = ((data[0]>>4)%2); |
tknara | 33:ef0d51f3aa23 | 67 | motor[pointedMotor].braking = (data[0]>>3)%2; |
tknara | 33:ef0d51f3aa23 | 68 | |
tknara | 33:ef0d51f3aa23 | 69 | motor[pointedMotor].setMode(mode[pointedMotor]); |
tknara | 33:ef0d51f3aa23 | 70 | if(data[1] == 126) { |
tknara | 33:ef0d51f3aa23 | 71 | motor[pointedMotor].setMotorSpeed(0); |
tknara | 33:ef0d51f3aa23 | 72 | //serial.printf("STOP"); |
tknara | 33:ef0d51f3aa23 | 73 | } else { |
tknara | 33:ef0d51f3aa23 | 74 | motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0); |
tknara | 33:ef0d51f3aa23 | 75 | } |
tknara | 33:ef0d51f3aa23 | 76 | addrChecked = false; |
tknara | 33:ef0d51f3aa23 | 77 | headerRecieved = false; |
tknara | 33:ef0d51f3aa23 | 78 | serial.putc(pointedMotor); |
tknara | 33:ef0d51f3aa23 | 79 | timeout.attach(&forceStop,TIMEOUT); |
tknara | 33:ef0d51f3aa23 | 80 | } |
tknara | 32:c64104c77531 | 81 | } |
tknara | 32:c64104c77531 | 82 | } |
tknara | 33:ef0d51f3aa23 | 83 | int main() |
tknara | 33:ef0d51f3aa23 | 84 | { |
tknara | 33:ef0d51f3aa23 | 85 | for(int i= 0; i< 4; i++) |
tknara | 33:ef0d51f3aa23 | 86 | motor[i].setMotorSpeed(0); |
tknara | 33:ef0d51f3aa23 | 87 | beep.period(1.0 / 2000.0); |
tknara | 33:ef0d51f3aa23 | 88 | beep = 0.6; |
tknara | 33:ef0d51f3aa23 | 89 | wait(0.1); |
tknara | 33:ef0d51f3aa23 | 90 | beep.period(1.0 / 4000.0); |
tknara | 33:ef0d51f3aa23 | 91 | beep = 0.6; |
tknara | 33:ef0d51f3aa23 | 92 | wait(0.1); |
tknara | 33:ef0d51f3aa23 | 93 | beep.period(1.0 / 6000.0); |
tknara | 33:ef0d51f3aa23 | 94 | beep = 0.6; |
tknara | 33:ef0d51f3aa23 | 95 | wait(0.1); |
tknara | 33:ef0d51f3aa23 | 96 | beep = 0.0; |
tknara | 33:ef0d51f3aa23 | 97 | wait(0.1); |
tknara | 33:ef0d51f3aa23 | 98 | beep.period(1.0 / 2000.0); |
tknara | 33:ef0d51f3aa23 | 99 | for(int i = 0; i<addr; i++) { |
tknara | 33:ef0d51f3aa23 | 100 | beep = 0.6; |
tknara | 33:ef0d51f3aa23 | 101 | wait(0.075); |
tknara | 33:ef0d51f3aa23 | 102 | beep = 0.0; |
tknara | 33:ef0d51f3aa23 | 103 | wait(0.075); |
tknara | 33:ef0d51f3aa23 | 104 | } // beep addr times |
tknara | 33:ef0d51f3aa23 | 105 | unsigned char tmp[3] = {0}; |
tknara | 33:ef0d51f3aa23 | 106 | addrChecked=false,headerRecieved=false; |
tknara | 33:ef0d51f3aa23 | 107 | rs485.baud(115200); |
tknara | 33:ef0d51f3aa23 | 108 | serial.baud(921600); |
tknara | 33:ef0d51f3aa23 | 109 | //addr.mode(PullUp); |
tknara | 33:ef0d51f3aa23 | 110 | RSControl = 0; |
tknara | 33:ef0d51f3aa23 | 111 | rs485.putc((1<<addr)); |
tknara | 33:ef0d51f3aa23 | 112 | rs485.attach(&callback); |
tknara | 33:ef0d51f3aa23 | 113 | beep = 0.0; |
tknara | 33:ef0d51f3aa23 | 114 | can.frequency(125000); |
tknara | 33:ef0d51f3aa23 | 115 | can.filter(addr,0x7ff,CANStandard); |
tknara | 33:ef0d51f3aa23 | 116 | serial.printf("addr:%d\n",addr); |
tknara | 33:ef0d51f3aa23 | 117 | wait(0.01); |
tknara | 32:c64104c77531 | 118 | can.attach(&canRxIt, CAN::RxIrq); |
hudakz | 0:c5e5d0df6f2a | 119 | while(1) { |
tknara | 33:ef0d51f3aa23 | 120 | debugLED = addr; |
tknara | 33:ef0d51f3aa23 | 121 | if(buf.size() > 4) { |
tknara | 33:ef0d51f3aa23 | 122 | if (buf[0] == 255) { |
tknara | 33:ef0d51f3aa23 | 123 | tmp[0] = buf[1]; |
tknara | 33:ef0d51f3aa23 | 124 | tmp[1] = buf[2]; |
tknara | 33:ef0d51f3aa23 | 125 | tmp[2] = buf[3]; |
tknara | 33:ef0d51f3aa23 | 126 | processData(tmp); |
tknara | 33:ef0d51f3aa23 | 127 | buf.erase(buf.begin(),buf.begin()+3); |
tknara | 33:ef0d51f3aa23 | 128 | debugLED0 = !debugLED0; |
tknara | 33:ef0d51f3aa23 | 129 | } else { |
tknara | 33:ef0d51f3aa23 | 130 | buf.erase(buf.begin()); |
tknara | 33:ef0d51f3aa23 | 131 | debugLED0 = false; |
tknara | 33:ef0d51f3aa23 | 132 | } |
tknara | 33:ef0d51f3aa23 | 133 | } |
tknara | 32:c64104c77531 | 134 | if(canRecieved) |
tknara | 32:c64104c77531 | 135 | { |
tknara | 33:ef0d51f3aa23 | 136 | for(int i=0;i<4;i++) |
tknara | 33:ef0d51f3aa23 | 137 | { |
tknara | 33:ef0d51f3aa23 | 138 | serial.printf("num:%d,data:%d",i,msg.data[i]); |
tknara | 33:ef0d51f3aa23 | 139 | motor[i].setMotorSpeed((msg.data[i]-126.0)/126.0); |
tknara | 33:ef0d51f3aa23 | 140 | } |
tknara | 33:ef0d51f3aa23 | 141 | serial.printf("\n"); |
tknara | 33:ef0d51f3aa23 | 142 | //processData(tmp); |
tknara | 32:c64104c77531 | 143 | debugLED0 = !debugLED0; |
tknara | 32:c64104c77531 | 144 | canRecieved = false; |
tknara | 32:c64104c77531 | 145 | } |
tknara | 33:ef0d51f3aa23 | 146 | if(estop) |
tknara | 33:ef0d51f3aa23 | 147 | forceStop(); |
tknara | 33:ef0d51f3aa23 | 148 | beep = estop*0.5; |
tknara | 33:ef0d51f3aa23 | 149 | //wait(0.001); |
hudakz | 0:c5e5d0df6f2a | 150 | } |
hudakz | 0:c5e5d0df6f2a | 151 | } |
hudakz | 24:e2907bcba75e | 152 | |
hudakz | 24:e2907bcba75e | 153 | |
hudakz | 24:e2907bcba75e | 154 |