NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

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

Dependencies:   mbed SoftPWM MotorSMLAP

main.cpp@4:2fb782334564, 2017-08-22 (annotated)

Committer:
WAT34
Date:
Tue Aug 22 02:10:11 2017 +0000
Revision:
4:2fb782334564
Parent:
3:22c5eb74817f
Child:
5:bcf70ca27a55
worked

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 4:2fb782334564 4
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 0:4791d1cb39f8 18 Timeout timeout;
WAT34 0:4791d1cb39f8 19 uint8_t pointedMotor;
WAT34 3:22c5eb74817f 20 bool estop =false,doubleHeader = false;
WAT34 4:2fb782334564 21 //BusIn addr(DIP_0,DIP_1,DIP_2);
WAT34 4:2fb782334564 22 uint8_t addr = 2;
WAT34 4:2fb782334564 23 DigitalOut beep(BUZER);
WAT34 3:22c5eb74817f 24
WAT34 4:2fb782334564 25 int mode[4] = {0};
WAT34 0:4791d1cb39f8 26
WAT34 0:4791d1cb39f8 27 void forceStop()
WAT34 0:4791d1cb39f8 28 {
eil4nyqn 1:d802a9793080 29 for(int i = 0;i < 1; i++)
WAT34 0:4791d1cb39f8 30 {
WAT34 0:4791d1cb39f8 31 motor[i].setMotorSpeed(0);
WAT34 0:4791d1cb39f8 32 }
WAT34 0:4791d1cb39f8 33 }
WAT34 2:de85eb142f32 34
WAT34 0:4791d1cb39f8 35 void callback(){
WAT34 4:2fb782334564 36 const uint8_t data = rs485.getc();
WAT34 4:2fb782334564 37 serial.putc(data);
WAT34 0:4791d1cb39f8 38 if(data == 255) //header
WAT34 0:4791d1cb39f8 39 {
WAT34 4:2fb782334564 40 if(headerRecieved){
WAT34 2:de85eb142f32 41 doubleHeader =true;
WAT34 4:2fb782334564 42 return;
WAT34 4:2fb782334564 43 }
WAT34 2:de85eb142f32 44 else if (doubleHeader)
WAT34 2:de85eb142f32 45 {
WAT34 2:de85eb142f32 46 estop = true;
WAT34 2:de85eb142f32 47 return;
WAT34 2:de85eb142f32 48 }
WAT34 0:4791d1cb39f8 49 addrChecked = false;
WAT34 0:4791d1cb39f8 50 headerRecieved = true;
WAT34 4:2fb782334564 51 return;
WAT34 0:4791d1cb39f8 52 }else
WAT34 0:4791d1cb39f8 53 if(headerRecieved && !addrChecked)
WAT34 0:4791d1cb39f8 54 {
WAT34 4:2fb782334564 55 //serial.printf("data :%d",data);
WAT34 2:de85eb142f32 56 doubleHeader=false;
WAT34 4:2fb782334564 57 if((data>>5) == addr)
WAT34 4:2fb782334564 58 {
WAT34 0:4791d1cb39f8 59 addrChecked =true;
WAT34 0:4791d1cb39f8 60 pointedMotor = data%4;
WAT34 4:2fb782334564 61 mode[pointedMotor] = (data>>4)%2;
WAT34 4:2fb782334564 62 //motor[pointedMotor].setMode(false);
WAT34 4:2fb782334564 63 RSControl = 1;
WAT34 4:2fb782334564 64 //rs485.putc(addr);
WAT34 4:2fb782334564 65 RSControl = 0;
WAT34 4:2fb782334564 66 }else{
WAT34 4:2fb782334564 67 headerRecieved = false;
WAT34 4:2fb782334564 68 addrChecked = false;
WAT34 0:4791d1cb39f8 69 }
WAT34 4:2fb782334564 70 return;
WAT34 2:de85eb142f32 71 }else if(headerRecieved && addrChecked && !estop)
WAT34 0:4791d1cb39f8 72 {
WAT34 4:2fb782334564 73 motor[pointedMotor].setMode(mode[pointedMotor]);
WAT34 2:de85eb142f32 74 doubleHeader = false;
WAT34 0:4791d1cb39f8 75 motor[pointedMotor].setMotorSpeed(data/126.0-1.0);
WAT34 0:4791d1cb39f8 76 addrChecked = false;
WAT34 0:4791d1cb39f8 77 headerRecieved = false;
WAT34 4:2fb782334564 78 serial.putc(pointedMotor);
eil4nyqn 1:d802a9793080 79 //timeout.attach(&forceStop,0.1);
WAT34 4:2fb782334564 80 }else{
WAT34 4:2fb782334564 81 doubleHeader = false;
WAT34 4:2fb782334564 82 headerRecieved =false;
WAT34 4:2fb782334564 83 addrChecked = false;
WAT34 0:4791d1cb39f8 84 }
WAT34 4:2fb782334564 85
WAT34 0:4791d1cb39f8 86 }
WAT34 0:4791d1cb39f8 87
WAT34 0:4791d1cb39f8 88 int main() {
WAT34 2:de85eb142f32 89 beep = 0;
eil4nyqn 1:d802a9793080 90 addrChecked=false,headerRecieved=false;
WAT34 4:2fb782334564 91 //motor[0].setMotorSpeed(0);
WAT34 4:2fb782334564 92 rs485.baud(115200);
eil4nyqn 1:d802a9793080 93 serial.baud(115200);
WAT34 4:2fb782334564 94 // addr.mode(PullUp);
WAT34 4:2fb782334564 95 RSControl = 0;
WAT34 4:2fb782334564 96 wait(0.5);
WAT34 4:2fb782334564 97 rs485.putc((1<<addr));
WAT34 4:2fb782334564 98 rs485.attach(&callback);
WAT34 4:2fb782334564 99
WAT34 0:4791d1cb39f8 100 while(1) {
WAT34 2:de85eb142f32 101 if(estop)
WAT34 2:de85eb142f32 102 forceStop();
WAT34 2:de85eb142f32 103 beep = estop;
WAT34 4:2fb782334564 104 wait(0.1);
WAT34 0:4791d1cb39f8 105 }
WAT34 0:4791d1cb39f8 106 }