NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

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

Dependencies:   mbed SoftPWM MotorSMLAP

main.cpp@9:1579e5cfa499, 2017-11-11 (annotated)

Committer:
WAT34
Date:
Sat Nov 11 00:52:25 2017 +0000
Revision:
9:1579e5cfa499
Parent:
8:fd749c243a8f
Parent:
7:5ebe1058ca5b
Child:
10:5f932239b8a5
marged

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"
Wataru Nakata 7:5ebe1058ca5b 4 #include <vector>
WAT34 6:eccd0b81ba62 5 #define TIMEOUT 0.5
WAT34 4:2fb782334564 6 using namespace pinConfig;
WAT34 9:1579e5cfa499 7 motorSmLap motor[]= {
WAT34 4:2fb782334564 8 motorSmLap(DIR_H_0,DIR_L_0,PWM0),
WAT34 4:2fb782334564 9 motorSmLap(DIR_H_1,DIR_L_1,PWM1),
WAT34 4:2fb782334564 10 motorSmLap(DIR_H_2,DIR_L_2,PWM2),
WAT34 4:2fb782334564 11 motorSmLap(DIR_H_3,DIR_L_3,PWM3)
WAT34 4:2fb782334564 12 };
WAT34 4:2fb782334564 13 Serial rs485(RS485_TX,RS485_RX);
WAT34 4:2fb782334564 14 Serial serial(UART1_TX,UART1_RX);
WAT34 4:2fb782334564 15
WAT34 4:2fb782334564 16 DigitalOut RSControl(RS485_CS);
Wataru Nakata 7:5ebe1058ca5b 17 bool addrChecked;
Wataru Nakata 7:5ebe1058ca5b 18 bool headerRecieved;
WAT34 9:1579e5cfa499 19 DigitalOut debugLED0(LED_0);
WAT34 9:1579e5cfa499 20 DigitalOut debugLED1(LED_1);
WAT34 5:bcf70ca27a55 21 BusOut debugLED(LED_2,LED_3);
WAT34 0:4791d1cb39f8 22 Timeout timeout;
WAT34 0:4791d1cb39f8 23 uint8_t pointedMotor;
WAT34 3:22c5eb74817f 24 bool estop =false,doubleHeader = false;
WAT34 5:bcf70ca27a55 25 BusIn addr(DIP_0,DIP_1,DIP_2);
WAT34 5:bcf70ca27a55 26 //uint8_t addr = 2;
WAT34 4:2fb782334564 27 DigitalOut beep(BUZER);
WAT34 5:bcf70ca27a55 28 bool brakeing[4] = {false};
WAT34 3:22c5eb74817f 29
WAT34 4:2fb782334564 30 int mode[4] = {0};
WAT34 0:4791d1cb39f8 31
Wataru Nakata 7:5ebe1058ca5b 32 std::vector<unsigned char> buf;
Wataru Nakata 7:5ebe1058ca5b 33
Wataru Nakata 7:5ebe1058ca5b 34
WAT34 0:4791d1cb39f8 35 void forceStop()
WAT34 0:4791d1cb39f8 36 {
WAT34 6:eccd0b81ba62 37 for(int i= 0; i< 4; i++)
WAT34 0:4791d1cb39f8 38 motor[i].setMotorSpeed(0);
WAT34 6:eccd0b81ba62 39 estop = true;
WAT34 0:4791d1cb39f8 40 }
WAT34 2:de85eb142f32 41
WAT34 9:1579e5cfa499 42 void callback()
WAT34 9:1579e5cfa499 43 {
WAT34 9:1579e5cfa499 44 debugLED0 != debugLED0;
Wataru Nakata 7:5ebe1058ca5b 45 buf.push_back(rs485.getc());
Wataru Nakata 7:5ebe1058ca5b 46 }
Wataru Nakata 7:5ebe1058ca5b 47
WAT34 9:1579e5cfa499 48 void processData(unsigned char data[])
Wataru Nakata 7:5ebe1058ca5b 49 {
WAT34 9:1579e5cfa499 50 if(data[0]^data[1] == data[2]) {
WAT34 9:1579e5cfa499 51 if((data[0]>>5) == addr) {
WAT34 0:4791d1cb39f8 52 addrChecked =true;
WAT34 9:1579e5cfa499 53 pointedMotor = data[0]%4;
WAT34 9:1579e5cfa499 54 mode[pointedMotor] = ((data[0]>>4)%2);
WAT34 9:1579e5cfa499 55 motor[pointedMotor].braking = (data[0]>>3)%2;
Wataru Nakata 7:5ebe1058ca5b 56
WAT34 9:1579e5cfa499 57 motor[pointedMotor].setMode(mode[pointedMotor]);
WAT34 9:1579e5cfa499 58 if(data[1] == 126) {
WAT34 9:1579e5cfa499 59 motor[pointedMotor].setMotorSpeed(0);
WAT34 9:1579e5cfa499 60 //serial.printf("STOP");
WAT34 9:1579e5cfa499 61 } else {
WAT34 9:1579e5cfa499 62 motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
WAT34 9:1579e5cfa499 63 }
WAT34 9:1579e5cfa499 64 addrChecked = false;
WAT34 4:2fb782334564 65 headerRecieved = false;
WAT34 9:1579e5cfa499 66 serial.putc(pointedMotor);
WAT34 9:1579e5cfa499 67 timeout.attach(&forceStop,TIMEOUT);
WAT34 5:bcf70ca27a55 68 }
WAT34 0:4791d1cb39f8 69 }
WAT34 0:4791d1cb39f8 70 }
WAT34 0:4791d1cb39f8 71
WAT34 9:1579e5cfa499 72
WAT34 9:1579e5cfa499 73
WAT34 9:1579e5cfa499 74 int main()
WAT34 9:1579e5cfa499 75 {
WAT34 6:eccd0b81ba62 76 for(int i= 0; i< 4; i++)
WAT34 6:eccd0b81ba62 77 motor[i].setMotorSpeed(0);
WAT34 5:bcf70ca27a55 78 beep = true;
WAT34 8:fd749c243a8f 79 wait(1);
WAT34 8:fd749c243a8f 80 beep = false;
WAT34 8:fd749c243a8f 81 wait(0.5);
WAT34 9:1579e5cfa499 82 for(int i = 0; i<=addr; i++) {
WAT34 8:fd749c243a8f 83 beep = true;
WAT34 8:fd749c243a8f 84 wait(0.1);
WAT34 8:fd749c243a8f 85 beep = false;
WAT34 8:fd749c243a8f 86 wait(0.1);
WAT34 8:fd749c243a8f 87 } // beep addr times
WAT34 9:1579e5cfa499 88
Wataru Nakata 7:5ebe1058ca5b 89 unsigned char tmp[3] = {0};
eil4nyqn 1:d802a9793080 90 addrChecked=false,headerRecieved=false;
WAT34 4:2fb782334564 91 //motor[0].setMotorSpeed(0);
WAT34 5:bcf70ca27a55 92 rs485.baud(38400);
eil4nyqn 1:d802a9793080 93 serial.baud(115200);
WAT34 5:bcf70ca27a55 94 addr.mode(PullUp);
WAT34 4:2fb782334564 95 RSControl = 0;
WAT34 4:2fb782334564 96 rs485.putc((1<<addr));
WAT34 4:2fb782334564 97 rs485.attach(&callback);
WAT34 5:bcf70ca27a55 98 beep = false;
WAT34 5:bcf70ca27a55 99 while(1) {
Wataru Nakata 7:5ebe1058ca5b 100 debugLED = addr;
WAT34 9:1579e5cfa499 101 if(buf.size() > 4) {
WAT34 9:1579e5cfa499 102 if (buf[0] == 255) {
WAT34 9:1579e5cfa499 103 tmp[0] = buf[1];
WAT34 9:1579e5cfa499 104 tmp[1] = buf[2];
WAT34 9:1579e5cfa499 105 tmp[2] = buf[3];
WAT34 9:1579e5cfa499 106 processData(tmp);
WAT34 9:1579e5cfa499 107 buf.erase(buf.begin(),buf.begin()+3);
WAT34 9:1579e5cfa499 108 } else {
WAT34 9:1579e5cfa499 109 buf.erase(buf.begin());
WAT34 9:1579e5cfa499 110 }
Wataru Nakata 7:5ebe1058ca5b 111 }
WAT34 2:de85eb142f32 112 if(estop)
WAT34 2:de85eb142f32 113 forceStop();
WAT34 2:de85eb142f32 114 beep = estop;
WAT34 4:2fb782334564 115 wait(0.1);
WAT34 0:4791d1cb39f8 116 }
WAT34 0:4791d1cb39f8 117 }