NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

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

Dependencies:   mbed SoftPWM MotorSMLAP

main.cpp@8:fd749c243a8f, 2017-11-11 (annotated)

Committer:
WAT34
Date:
Sat Nov 11 00:43:54 2017 +0000
Revision:
8:fd749c243a8f
Parent:
6:eccd0b81ba62
Child:
9:1579e5cfa499
beep beep beep

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