NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

ikarashiMDCslaveの改良(?)版　スピード制御の分解能が2byte

Dependencies:   mbed SoftPWM MotorSMLAP

main.cpp@12:3a734542a2b3, 2018-11-27 (annotated)

Committer:
tknara
Date:
Tue Nov 27 06:17:21 2018 +0000
Revision:
12:3a734542a2b3
Parent:
11:3ef351087c94
Child:
13:0cd5e8c91941
add ID check

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 0:4791d1cb39f8 1 #include "mbed.h"
WAT34 0:4791d1cb39f8 2 #include "motorsmlap.h"
WAT34 4:2fb782334564 3 #include "MDC3_0pinConfig.h"
takeuchi 10:5f932239b8a5 4 #include "SoftPWM.h"
Wataru Nakata 7:5ebe1058ca5b 5 #include <vector>
WAT34 6:eccd0b81ba62 6 #define TIMEOUT 0.5
WAT34 4:2fb782334564 7 using namespace pinConfig;
WAT34 9:1579e5cfa499 8 motorSmLap motor[]= {
WAT34 4:2fb782334564 9 motorSmLap(DIR_H_0,DIR_L_0,PWM0),
WAT34 4:2fb782334564 10 motorSmLap(DIR_H_1,DIR_L_1,PWM1),
WAT34 4:2fb782334564 11 motorSmLap(DIR_H_2,DIR_L_2,PWM2),
WAT34 4:2fb782334564 12 motorSmLap(DIR_H_3,DIR_L_3,PWM3)
WAT34 4:2fb782334564 13 };
WAT34 4:2fb782334564 14 Serial rs485(RS485_TX,RS485_RX);
WAT34 4:2fb782334564 15 Serial serial(UART1_TX,UART1_RX);
WAT34 11:3ef351087c94 16 CAN can(CAN_RX, CAN_TX);
WAT34 4:2fb782334564 17
WAT34 4:2fb782334564 18 DigitalOut RSControl(RS485_CS);
Wataru Nakata 7:5ebe1058ca5b 19 bool addrChecked;
Wataru Nakata 7:5ebe1058ca5b 20 bool headerRecieved;
WAT34 9:1579e5cfa499 21 DigitalOut debugLED0(LED_0);
WAT34 9:1579e5cfa499 22 DigitalOut debugLED1(LED_1);
WAT34 5:bcf70ca27a55 23 BusOut debugLED(LED_2,LED_3);
WAT34 0:4791d1cb39f8 24 Timeout timeout;
WAT34 0:4791d1cb39f8 25 uint8_t pointedMotor;
WAT34 3:22c5eb74817f 26 bool estop =false,doubleHeader = false;
WAT34 5:bcf70ca27a55 27 BusIn addr(DIP_0,DIP_1,DIP_2);
WAT34 5:bcf70ca27a55 28 //uint8_t addr = 2;
takeuchi 10:5f932239b8a5 29 SoftPWM beep(BUZER);
WAT34 5:bcf70ca27a55 30 bool brakeing[4] = {false};
WAT34 11:3ef351087c94 31 bool canRecieved = false;
WAT34 3:22c5eb74817f 32
WAT34 4:2fb782334564 33 int mode[4] = {0};
WAT34 0:4791d1cb39f8 34
Wataru Nakata 7:5ebe1058ca5b 35 std::vector<unsigned char> buf;
WAT34 11:3ef351087c94 36 uint8_t canbuf[3];
WAT34 11:3ef351087c94 37 CANMessage msg;
Wataru Nakata 7:5ebe1058ca5b 38
WAT34 11:3ef351087c94 39 void canRxIt()
WAT34 11:3ef351087c94 40 {
tknara 12:3a734542a2b3 41 if(can.read(msg)&&(msg.id==addr))
tknara 12:3a734542a2b3 42 {
WAT34 11:3ef351087c94 43 canbuf[0] = msg.data[0];
WAT34 11:3ef351087c94 44 canbuf[1] = msg.data[1];
WAT34 11:3ef351087c94 45 canbuf[2] = msg.data[2];
WAT34 11:3ef351087c94 46 canRecieved = true;
WAT34 11:3ef351087c94 47 }
WAT34 11:3ef351087c94 48 }
Wataru Nakata 7:5ebe1058ca5b 49
WAT34 0:4791d1cb39f8 50 void forceStop()
WAT34 0:4791d1cb39f8 51 {
WAT34 6:eccd0b81ba62 52 for(int i= 0; i< 4; i++)
WAT34 0:4791d1cb39f8 53 motor[i].setMotorSpeed(0);
WAT34 6:eccd0b81ba62 54 estop = true;
WAT34 0:4791d1cb39f8 55 }
WAT34 2:de85eb142f32 56
WAT34 9:1579e5cfa499 57 void callback()
WAT34 9:1579e5cfa499 58 {
Wataru Nakata 7:5ebe1058ca5b 59 buf.push_back(rs485.getc());
Wataru Nakata 7:5ebe1058ca5b 60 }
Wataru Nakata 7:5ebe1058ca5b 61
WAT34 9:1579e5cfa499 62 void processData(unsigned char data[])
Wataru Nakata 7:5ebe1058ca5b 63 {
WAT34 9:1579e5cfa499 64 if(data[0]^data[1] == data[2]) {
WAT34 9:1579e5cfa499 65 if((data[0]>>5) == addr) {
WAT34 0:4791d1cb39f8 66 addrChecked =true;
WAT34 9:1579e5cfa499 67 pointedMotor = data[0]%4;
WAT34 9:1579e5cfa499 68 mode[pointedMotor] = ((data[0]>>4)%2);
WAT34 9:1579e5cfa499 69 motor[pointedMotor].braking = (data[0]>>3)%2;
Wataru Nakata 7:5ebe1058ca5b 70
WAT34 9:1579e5cfa499 71 motor[pointedMotor].setMode(mode[pointedMotor]);
WAT34 9:1579e5cfa499 72 if(data[1] == 126) {
WAT34 9:1579e5cfa499 73 motor[pointedMotor].setMotorSpeed(0);
WAT34 9:1579e5cfa499 74 //serial.printf("STOP");
WAT34 9:1579e5cfa499 75 } else {
WAT34 9:1579e5cfa499 76 motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
WAT34 9:1579e5cfa499 77 }
WAT34 9:1579e5cfa499 78 addrChecked = false;
WAT34 4:2fb782334564 79 headerRecieved = false;
WAT34 9:1579e5cfa499 80 serial.putc(pointedMotor);
WAT34 9:1579e5cfa499 81 timeout.attach(&forceStop,TIMEOUT);
WAT34 5:bcf70ca27a55 82 }
WAT34 0:4791d1cb39f8 83 }
WAT34 0:4791d1cb39f8 84 }
WAT34 0:4791d1cb39f8 85
WAT34 9:1579e5cfa499 86
WAT34 9:1579e5cfa499 87
WAT34 9:1579e5cfa499 88 int main()
WAT34 9:1579e5cfa499 89 {
WAT34 6:eccd0b81ba62 90 for(int i= 0; i< 4; i++)
WAT34 6:eccd0b81ba62 91 motor[i].setMotorSpeed(0);
takeuchi 10:5f932239b8a5 92 beep.period(1.0 / 2000.0);
takeuchi 10:5f932239b8a5 93 beep = 0.6;
takeuchi 10:5f932239b8a5 94 wait(0.1);
takeuchi 10:5f932239b8a5 95 beep.period(1.0 / 4000.0);
takeuchi 10:5f932239b8a5 96 beep = 0.6;
takeuchi 10:5f932239b8a5 97 wait(0.1);
takeuchi 10:5f932239b8a5 98 beep.period(1.0 / 6000.0);
takeuchi 10:5f932239b8a5 99 beep = 0.6;
takeuchi 10:5f932239b8a5 100 wait(0.1);
takeuchi 10:5f932239b8a5 101 beep = 0.0;
takeuchi 10:5f932239b8a5 102 wait(0.1);
takeuchi 10:5f932239b8a5 103 beep.period(1.0 / 2000.0);
takeuchi 10:5f932239b8a5 104 for(int i = 0; i<addr; i++) {
takeuchi 10:5f932239b8a5 105 beep = 0.6;
takeuchi 10:5f932239b8a5 106 wait(0.075);
takeuchi 10:5f932239b8a5 107 beep = 0.0;
takeuchi 10:5f932239b8a5 108 wait(0.075);
WAT34 8:fd749c243a8f 109 } // beep addr times
WAT34 9:1579e5cfa499 110
Wataru Nakata 7:5ebe1058ca5b 111 unsigned char tmp[3] = {0};
eil4nyqn 1:d802a9793080 112 addrChecked=false,headerRecieved=false;
WAT34 11:3ef351087c94 113 can.attach(&canRxIt, CAN::RxIrq);
WAT34 4:2fb782334564 114 //motor[0].setMotorSpeed(0);
takeuchi 10:5f932239b8a5 115 rs485.baud(115200);
eil4nyqn 1:d802a9793080 116 serial.baud(115200);
WAT34 5:bcf70ca27a55 117 addr.mode(PullUp);
WAT34 4:2fb782334564 118 RSControl = 0;
WAT34 4:2fb782334564 119 rs485.putc((1<<addr));
WAT34 4:2fb782334564 120 rs485.attach(&callback);
takeuchi 10:5f932239b8a5 121 beep = 0.0;
WAT34 5:bcf70ca27a55 122 while(1) {
Wataru Nakata 7:5ebe1058ca5b 123 debugLED = addr;
WAT34 9:1579e5cfa499 124 if(buf.size() > 4) {
WAT34 9:1579e5cfa499 125 if (buf[0] == 255) {
WAT34 9:1579e5cfa499 126 tmp[0] = buf[1];
WAT34 9:1579e5cfa499 127 tmp[1] = buf[2];
WAT34 9:1579e5cfa499 128 tmp[2] = buf[3];
WAT34 9:1579e5cfa499 129 processData(tmp);
WAT34 9:1579e5cfa499 130 buf.erase(buf.begin(),buf.begin()+3);
takeuchi 10:5f932239b8a5 131 debugLED0 = !debugLED0;
WAT34 9:1579e5cfa499 132 } else {
WAT34 9:1579e5cfa499 133 buf.erase(buf.begin());
takeuchi 10:5f932239b8a5 134 debugLED0 = false;
WAT34 9:1579e5cfa499 135 }
Wataru Nakata 7:5ebe1058ca5b 136 }
WAT34 11:3ef351087c94 137 if(canRecieved)
WAT34 11:3ef351087c94 138 {
WAT34 11:3ef351087c94 139 tmp[0] = canbuf[0];
WAT34 11:3ef351087c94 140 tmp[1] = canbuf[1];
WAT34 11:3ef351087c94 141 tmp[2] = canbuf[2];
WAT34 11:3ef351087c94 142 processData(tmp);
WAT34 11:3ef351087c94 143 debugLED0 = !debugLED0;
WAT34 11:3ef351087c94 144 canRecieved = false;
WAT34 11:3ef351087c94 145 }
WAT34 2:de85eb142f32 146 if(estop)
WAT34 2:de85eb142f32 147 forceStop();
WAT34 2:de85eb142f32 148 beep = estop;
takeuchi 10:5f932239b8a5 149 //wait(0.1);
WAT34 0:4791d1cb39f8 150 }
WAT34 0:4791d1cb39f8 151 }