NagaokaRoboticsClub_mbedTeam / CanTestf103_slave

Can test program.Maybe filter is not implemented

Dependencies:   SoftPWM MotorSMLAP

Fork of CANnucleo_Hello by Zoltan Hudak

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?

UserRevisionLine numberNew 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