NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

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

Dependencies:   mbed SoftPWM MotorSMLAP

main.cpp@13:0cd5e8c91941, 2018-11-27 (annotated)

Committer:
WAT34
Date:
Tue Nov 27 08:01:19 2018 +0000
Revision:
13:0cd5e8c91941
Parent:
12:3a734542a2b3
Child:
14:f3d19afbb99e
test added

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;
WAT34 11:3ef351087c94 38 uint8_t canbuf[3];
WAT34 11:3ef351087c94 39 CANMessage msg;
WAT34 13:0cd5e8c91941 40 bool testing = false;
WAT34 13:0cd5e8c91941 41 void runTest()
WAT34 13:0cd5e8c91941 42 {
WAT34 13:0cd5e8c91941 43 testing = true;
WAT34 13:0cd5e8c91941 44 //beep = 1;
WAT34 13:0cd5e8c91941 45 for(int i = 0;i < 4;i++)
WAT34 13:0cd5e8c91941 46 {
WAT34 13:0cd5e8c91941 47 motor[i].setFreq(1600*i);
WAT34 13:0cd5e8c91941 48 motor[i].setMotorSpeed(0.1);
WAT34 13:0cd5e8c91941 49 wait(0.3);
WAT34 13:0cd5e8c91941 50 motor[i].setMotorSpeed(0);
WAT34 13:0cd5e8c91941 51 motor[i].setFreq(20000.0);
WAT34 13:0cd5e8c91941 52 }
WAT34 13:0cd5e8c91941 53 testing = false;
WAT34 13:0cd5e8c91941 54 //beep = 0;
WAT34 13:0cd5e8c91941 55 }
Wataru Nakata 7:5ebe1058ca5b 56
WAT34 11:3ef351087c94 57 void canRxIt()
WAT34 11:3ef351087c94 58 {
tknara 12:3a734542a2b3 59 if(can.read(msg)&&(msg.id==addr))
tknara 12:3a734542a2b3 60 {
WAT34 11:3ef351087c94 61 canbuf[0] = msg.data[0];
WAT34 11:3ef351087c94 62 canbuf[1] = msg.data[1];
WAT34 11:3ef351087c94 63 canbuf[2] = msg.data[2];
WAT34 11:3ef351087c94 64 canRecieved = true;
WAT34 11:3ef351087c94 65 }
WAT34 11:3ef351087c94 66 }
Wataru Nakata 7:5ebe1058ca5b 67
WAT34 0:4791d1cb39f8 68 void forceStop()
WAT34 0:4791d1cb39f8 69 {
WAT34 13:0cd5e8c91941 70 if(testing)
WAT34 13:0cd5e8c91941 71 return;
WAT34 6:eccd0b81ba62 72 for(int i= 0; i< 4; i++)
WAT34 0:4791d1cb39f8 73 motor[i].setMotorSpeed(0);
WAT34 6:eccd0b81ba62 74 estop = true;
WAT34 0:4791d1cb39f8 75 }
WAT34 2:de85eb142f32 76
WAT34 9:1579e5cfa499 77 void callback()
WAT34 9:1579e5cfa499 78 {
Wataru Nakata 7:5ebe1058ca5b 79 buf.push_back(rs485.getc());
Wataru Nakata 7:5ebe1058ca5b 80 }
Wataru Nakata 7:5ebe1058ca5b 81
WAT34 9:1579e5cfa499 82 void processData(unsigned char data[])
Wataru Nakata 7:5ebe1058ca5b 83 {
WAT34 9:1579e5cfa499 84 if(data[0]^data[1] == data[2]) {
WAT34 9:1579e5cfa499 85 if((data[0]>>5) == addr) {
WAT34 0:4791d1cb39f8 86 addrChecked =true;
WAT34 9:1579e5cfa499 87 pointedMotor = data[0]%4;
WAT34 9:1579e5cfa499 88 mode[pointedMotor] = ((data[0]>>4)%2);
WAT34 9:1579e5cfa499 89 motor[pointedMotor].braking = (data[0]>>3)%2;
Wataru Nakata 7:5ebe1058ca5b 90
WAT34 9:1579e5cfa499 91 motor[pointedMotor].setMode(mode[pointedMotor]);
WAT34 9:1579e5cfa499 92 if(data[1] == 126) {
WAT34 9:1579e5cfa499 93 motor[pointedMotor].setMotorSpeed(0);
WAT34 9:1579e5cfa499 94 //serial.printf("STOP");
WAT34 9:1579e5cfa499 95 } else {
WAT34 9:1579e5cfa499 96 motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
WAT34 9:1579e5cfa499 97 }
WAT34 9:1579e5cfa499 98 addrChecked = false;
WAT34 4:2fb782334564 99 headerRecieved = false;
WAT34 9:1579e5cfa499 100 serial.putc(pointedMotor);
WAT34 9:1579e5cfa499 101 timeout.attach(&forceStop,TIMEOUT);
WAT34 5:bcf70ca27a55 102 }
WAT34 0:4791d1cb39f8 103 }
WAT34 0:4791d1cb39f8 104 }
WAT34 0:4791d1cb39f8 105
WAT34 9:1579e5cfa499 106
WAT34 9:1579e5cfa499 107
WAT34 9:1579e5cfa499 108 int main()
WAT34 9:1579e5cfa499 109 {
WAT34 6:eccd0b81ba62 110 for(int i= 0; i< 4; i++)
WAT34 13:0cd5e8c91941 111 {
WAT34 13:0cd5e8c91941 112 motor[i].setMode(SM);
WAT34 6:eccd0b81ba62 113 motor[i].setMotorSpeed(0);
WAT34 13:0cd5e8c91941 114 }
takeuchi 10:5f932239b8a5 115 beep.period(1.0 / 2000.0);
takeuchi 10:5f932239b8a5 116 beep = 0.6;
takeuchi 10:5f932239b8a5 117 wait(0.1);
takeuchi 10:5f932239b8a5 118 beep.period(1.0 / 4000.0);
takeuchi 10:5f932239b8a5 119 beep = 0.6;
takeuchi 10:5f932239b8a5 120 wait(0.1);
takeuchi 10:5f932239b8a5 121 beep.period(1.0 / 6000.0);
takeuchi 10:5f932239b8a5 122 beep = 0.6;
takeuchi 10:5f932239b8a5 123 wait(0.1);
takeuchi 10:5f932239b8a5 124 beep = 0.0;
takeuchi 10:5f932239b8a5 125 wait(0.1);
takeuchi 10:5f932239b8a5 126 beep.period(1.0 / 2000.0);
takeuchi 10:5f932239b8a5 127 for(int i = 0; i<addr; i++) {
takeuchi 10:5f932239b8a5 128 beep = 0.6;
takeuchi 10:5f932239b8a5 129 wait(0.075);
takeuchi 10:5f932239b8a5 130 beep = 0.0;
takeuchi 10:5f932239b8a5 131 wait(0.075);
WAT34 8:fd749c243a8f 132 } // beep addr times
WAT34 9:1579e5cfa499 133
Wataru Nakata 7:5ebe1058ca5b 134 unsigned char tmp[3] = {0};
eil4nyqn 1:d802a9793080 135 addrChecked=false,headerRecieved=false;
WAT34 11:3ef351087c94 136 can.attach(&canRxIt, CAN::RxIrq);
WAT34 4:2fb782334564 137 //motor[0].setMotorSpeed(0);
takeuchi 10:5f932239b8a5 138 rs485.baud(115200);
eil4nyqn 1:d802a9793080 139 serial.baud(115200);
WAT34 5:bcf70ca27a55 140 addr.mode(PullUp);
WAT34 13:0cd5e8c91941 141 test1.mode(PullDown);
WAT34 13:0cd5e8c91941 142 test2.mode(PullDown);
WAT34 4:2fb782334564 143 RSControl = 0;
WAT34 4:2fb782334564 144 rs485.putc((1<<addr));
WAT34 4:2fb782334564 145 rs485.attach(&callback);
takeuchi 10:5f932239b8a5 146 beep = 0.0;
WAT34 5:bcf70ca27a55 147 while(1) {
Wataru Nakata 7:5ebe1058ca5b 148 debugLED = addr;
WAT34 9:1579e5cfa499 149 if(buf.size() > 4) {
WAT34 9:1579e5cfa499 150 if (buf[0] == 255) {
WAT34 9:1579e5cfa499 151 tmp[0] = buf[1];
WAT34 9:1579e5cfa499 152 tmp[1] = buf[2];
WAT34 9:1579e5cfa499 153 tmp[2] = buf[3];
WAT34 9:1579e5cfa499 154 processData(tmp);
WAT34 9:1579e5cfa499 155 buf.erase(buf.begin(),buf.begin()+3);
takeuchi 10:5f932239b8a5 156 debugLED0 = !debugLED0;
WAT34 9:1579e5cfa499 157 } else {
WAT34 9:1579e5cfa499 158 buf.erase(buf.begin());
takeuchi 10:5f932239b8a5 159 debugLED0 = false;
WAT34 9:1579e5cfa499 160 }
Wataru Nakata 7:5ebe1058ca5b 161 }
WAT34 11:3ef351087c94 162 if(canRecieved)
WAT34 11:3ef351087c94 163 {
WAT34 11:3ef351087c94 164 tmp[0] = canbuf[0];
WAT34 11:3ef351087c94 165 tmp[1] = canbuf[1];
WAT34 11:3ef351087c94 166 tmp[2] = canbuf[2];
WAT34 11:3ef351087c94 167 processData(tmp);
WAT34 11:3ef351087c94 168 debugLED0 = !debugLED0;
WAT34 11:3ef351087c94 169 canRecieved = false;
WAT34 11:3ef351087c94 170 }
WAT34 2:de85eb142f32 171 if(estop)
WAT34 2:de85eb142f32 172 forceStop();
WAT34 2:de85eb142f32 173 beep = estop;
WAT34 13:0cd5e8c91941 174
WAT34 13:0cd5e8c91941 175 if(test1 && test2)
WAT34 13:0cd5e8c91941 176 runTest();
WAT34 0:4791d1cb39f8 177 }
WAT34 0:4791d1cb39f8 178 }