NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

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

Dependencies:   mbed SoftPWM MotorSMLAP

main.cpp@18:a8e9a14dbcfa, 2019-09-03 (annotated)

Committer:
skouki
Date:
Tue Sep 03 04:31:55 2019 +0000
Revision:
18:a8e9a14dbcfa
Parent:
17:1202d7305eab 
;

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 13:0cd5e8c91941 21 DigitalIn test1(pinConfig::I2C_SDA);
WAT34 13:0cd5e8c91941 22 DigitalIn test2(pinConfig::I2C_SCL);
WAT34 9:1579e5cfa499 23 DigitalOut debugLED0(LED_0);
WAT34 9:1579e5cfa499 24 DigitalOut debugLED1(LED_1);
WAT34 5:bcf70ca27a55 25 BusOut debugLED(LED_2,LED_3);
WAT34 0:4791d1cb39f8 26 Timeout timeout;
WAT34 0:4791d1cb39f8 27 uint8_t pointedMotor;
WAT34 3:22c5eb74817f 28 bool estop =false,doubleHeader = false;
WAT34 5:bcf70ca27a55 29 BusIn addr(DIP_0,DIP_1,DIP_2);
WAT34 5:bcf70ca27a55 30 //uint8_t addr = 2;
takeuchi 10:5f932239b8a5 31 SoftPWM beep(BUZER);
WAT34 5:bcf70ca27a55 32 bool brakeing[4] = {false};
WAT34 11:3ef351087c94 33 bool canRecieved = false;
WAT34 3:22c5eb74817f 34
WAT34 4:2fb782334564 35 int mode[4] = {0};
WAT34 0:4791d1cb39f8 36
Wataru Nakata 7:5ebe1058ca5b 37 std::vector<unsigned char> buf;
skouki 15:0db5eea67306 38 uint8_t canbuf[4];
WAT34 11:3ef351087c94 39 CANMessage msg;
WAT34 13:0cd5e8c91941 40 bool testing = false;
skouki 14:f3d19afbb99e 41 void start();
skouki 14:f3d19afbb99e 42
WAT34 13:0cd5e8c91941 43 void runTest()
WAT34 13:0cd5e8c91941 44 {
WAT34 13:0cd5e8c91941 45 testing = true;
WAT34 13:0cd5e8c91941 46 //beep = 1;
WAT34 13:0cd5e8c91941 47 for(int i = 0;i < 4;i++)
WAT34 13:0cd5e8c91941 48 {
WAT34 13:0cd5e8c91941 49 motor[i].setFreq(1600*i);
WAT34 13:0cd5e8c91941 50 motor[i].setMotorSpeed(0.1);
WAT34 13:0cd5e8c91941 51 wait(0.3);
WAT34 13:0cd5e8c91941 52 motor[i].setMotorSpeed(0);
WAT34 13:0cd5e8c91941 53 motor[i].setFreq(20000.0);
WAT34 13:0cd5e8c91941 54 }
WAT34 13:0cd5e8c91941 55 testing = false;
WAT34 13:0cd5e8c91941 56 //beep = 0;
WAT34 13:0cd5e8c91941 57 }
Wataru Nakata 7:5ebe1058ca5b 58
WAT34 11:3ef351087c94 59 void canRxIt()
WAT34 11:3ef351087c94 60 {
tknara 12:3a734542a2b3 61 if(can.read(msg)&&(msg.id==addr))
tknara 12:3a734542a2b3 62 {
WAT34 11:3ef351087c94 63 canbuf[0] = msg.data[0];
WAT34 11:3ef351087c94 64 canbuf[1] = msg.data[1];
WAT34 11:3ef351087c94 65 canbuf[2] = msg.data[2];
skouki 15:0db5eea67306 66 canbuf[3] = msg.data[3];
WAT34 11:3ef351087c94 67 canRecieved = true;
WAT34 11:3ef351087c94 68 }
WAT34 11:3ef351087c94 69 }
Wataru Nakata 7:5ebe1058ca5b 70
WAT34 0:4791d1cb39f8 71 void forceStop()
WAT34 0:4791d1cb39f8 72 {
WAT34 13:0cd5e8c91941 73 if(testing)
WAT34 13:0cd5e8c91941 74 return;
WAT34 6:eccd0b81ba62 75 for(int i= 0; i< 4; i++)
WAT34 0:4791d1cb39f8 76 motor[i].setMotorSpeed(0);
WAT34 6:eccd0b81ba62 77 estop = true;
skouki 14:f3d19afbb99e 78
WAT34 0:4791d1cb39f8 79 }
WAT34 2:de85eb142f32 80
WAT34 9:1579e5cfa499 81 void callback()
WAT34 9:1579e5cfa499 82 {
Wataru Nakata 7:5ebe1058ca5b 83 buf.push_back(rs485.getc());
Wataru Nakata 7:5ebe1058ca5b 84 }
Wataru Nakata 7:5ebe1058ca5b 85
WAT34 9:1579e5cfa499 86 void processData(unsigned char data[])
Wataru Nakata 7:5ebe1058ca5b 87 {
skouki 15:0db5eea67306 88 if(data[0]^data[1]^data[2] == data[3]) {
WAT34 9:1579e5cfa499 89 if((data[0]>>5) == addr) {
WAT34 0:4791d1cb39f8 90 addrChecked =true;
WAT34 9:1579e5cfa499 91 pointedMotor = data[0]%4;
WAT34 9:1579e5cfa499 92 mode[pointedMotor] = ((data[0]>>4)%2);
WAT34 9:1579e5cfa499 93 motor[pointedMotor].braking = (data[0]>>3)%2;
Wataru Nakata 7:5ebe1058ca5b 94
WAT34 9:1579e5cfa499 95 motor[pointedMotor].setMode(mode[pointedMotor]);
skouki 15:0db5eea67306 96 if((data[1] + (data[2]<<8)) == 32768) {
skouki 17:1202d7305eab 97 motor[pointedMotor].setMotorSpeed(0.0);
WAT34 9:1579e5cfa499 98 //serial.printf("STOP");
WAT34 9:1579e5cfa499 99 } else {
skouki 15:0db5eea67306 100 motor[pointedMotor].setMotorSpeed(((data[1] + (data[2]<<8))-32768.0)/32768.0);
WAT34 9:1579e5cfa499 101 }
WAT34 9:1579e5cfa499 102 addrChecked = false;
WAT34 4:2fb782334564 103 headerRecieved = false;
WAT34 9:1579e5cfa499 104 serial.putc(pointedMotor);
WAT34 9:1579e5cfa499 105 timeout.attach(&forceStop,TIMEOUT);
WAT34 5:bcf70ca27a55 106 }
WAT34 0:4791d1cb39f8 107 }
WAT34 0:4791d1cb39f8 108 }
WAT34 0:4791d1cb39f8 109
WAT34 9:1579e5cfa499 110
skouki 14:f3d19afbb99e 111 void start()
WAT34 9:1579e5cfa499 112 {
WAT34 6:eccd0b81ba62 113 for(int i= 0; i< 4; i++)
WAT34 13:0cd5e8c91941 114 {
WAT34 13:0cd5e8c91941 115 motor[i].setMode(SM);
WAT34 6:eccd0b81ba62 116 motor[i].setMotorSpeed(0);
WAT34 13:0cd5e8c91941 117 }
skouki 16:8c2eb2b4bead 118 beep.period(1.0 / 2959.955);
skouki 16:8c2eb2b4bead 119 beep = 0.2;
skouki 14:f3d19afbb99e 120 wait(0.4);
skouki 14:f3d19afbb99e 121
skouki 14:f3d19afbb99e 122 beep = 0.0;
takeuchi 10:5f932239b8a5 123 wait(0.1);
skouki 14:f3d19afbb99e 124
skouki 16:8c2eb2b4bead 125 beep.period(1.0 / 2637.020);
skouki 16:8c2eb2b4bead 126 beep = 0.2;
skouki 16:8c2eb2b4bead 127 wait(0.4);
skouki 14:f3d19afbb99e 128
takeuchi 10:5f932239b8a5 129 beep = 0.0;
takeuchi 10:5f932239b8a5 130 wait(0.1);
skouki 14:f3d19afbb99e 131
skouki 16:8c2eb2b4bead 132 beep.period(1.0 / 3322.438);
takeuchi 10:5f932239b8a5 133 for(int i = 0; i<addr; i++) {
skouki 16:8c2eb2b4bead 134 beep = 0.2;
skouki 16:8c2eb2b4bead 135 wait(0.4);
takeuchi 10:5f932239b8a5 136 beep = 0.0;
takeuchi 10:5f932239b8a5 137 wait(0.075);
skouki 14:f3d19afbb99e 138 } // beep addr times
skouki 14:f3d19afbb99e 139 beep.period(1.0 / 1000.0);
skouki 14:f3d19afbb99e 140
skouki 14:f3d19afbb99e 141 }
skouki 14:f3d19afbb99e 142 int main()
skouki 14:f3d19afbb99e 143 {
skouki 14:f3d19afbb99e 144 start();
WAT34 9:1579e5cfa499 145
skouki 15:0db5eea67306 146 unsigned char tmp[4] = {0};
eil4nyqn 1:d802a9793080 147 addrChecked=false,headerRecieved=false;
WAT34 11:3ef351087c94 148 can.attach(&canRxIt, CAN::RxIrq);
WAT34 4:2fb782334564 149 //motor[0].setMotorSpeed(0);
takeuchi 10:5f932239b8a5 150 rs485.baud(115200);
eil4nyqn 1:d802a9793080 151 serial.baud(115200);
WAT34 5:bcf70ca27a55 152 addr.mode(PullUp);
WAT34 13:0cd5e8c91941 153 test1.mode(PullDown);
WAT34 13:0cd5e8c91941 154 test2.mode(PullDown);
WAT34 4:2fb782334564 155 RSControl = 0;
WAT34 4:2fb782334564 156 rs485.putc((1<<addr));
WAT34 4:2fb782334564 157 rs485.attach(&callback);
takeuchi 10:5f932239b8a5 158 beep = 0.0;
WAT34 5:bcf70ca27a55 159 while(1) {
Wataru Nakata 7:5ebe1058ca5b 160 debugLED = addr;
skouki 18:a8e9a14dbcfa 161 debugLED1 = !debugLED1;
skouki 15:0db5eea67306 162 if(buf.size() > 5) {
skouki 17:1202d7305eab 163 if ((buf[0] == 255)&&buf[1] == 254) {
skouki 17:1202d7305eab 164 tmp[0] = buf[2];
skouki 17:1202d7305eab 165 tmp[1] = buf[3];
skouki 17:1202d7305eab 166 tmp[2] = buf[4];
skouki 17:1202d7305eab 167 tmp[3] = buf[5];
WAT34 9:1579e5cfa499 168 processData(tmp);
skouki 17:1202d7305eab 169 buf.erase(buf.begin(),buf.begin()+5);
skouki 17:1202d7305eab 170 debugLED0 = true;
WAT34 9:1579e5cfa499 171 } else {
WAT34 9:1579e5cfa499 172 buf.erase(buf.begin());
takeuchi 10:5f932239b8a5 173 debugLED0 = false;
WAT34 9:1579e5cfa499 174 }
Wataru Nakata 7:5ebe1058ca5b 175 }
WAT34 11:3ef351087c94 176 if(canRecieved)
WAT34 11:3ef351087c94 177 {
WAT34 11:3ef351087c94 178 tmp[0] = canbuf[0];
WAT34 11:3ef351087c94 179 tmp[1] = canbuf[1];
WAT34 11:3ef351087c94 180 tmp[2] = canbuf[2];
skouki 15:0db5eea67306 181 tmp[3] = canbuf[3];
WAT34 11:3ef351087c94 182 processData(tmp);
WAT34 11:3ef351087c94 183 debugLED0 = !debugLED0;
WAT34 11:3ef351087c94 184 canRecieved = false;
WAT34 11:3ef351087c94 185 }
WAT34 2:de85eb142f32 186 if(estop)
WAT34 2:de85eb142f32 187 forceStop();
WAT34 2:de85eb142f32 188 beep = estop;
WAT34 13:0cd5e8c91941 189
WAT34 13:0cd5e8c91941 190 if(test1 && test2)
WAT34 13:0cd5e8c91941 191 runTest();
WAT34 0:4791d1cb39f8 192 }
WAT34 0:4791d1cb39f8 193 }