albatross / Mbed 2 deprecated Souda_Main_ver3

3/18 操舵中央

Dependencies:   mbed

Fork of Control_Main_Full_ver2 by albatross

main.cpp@2:9dc7d5f1e910, 2016-02-16 (annotated)

Committer:
taurin
Date:
Tue Feb 16 09:50:45 2016 +0000
Revision:
2:9dc7d5f1e910
Parent:
1:65a3a2a4f535
Child:
3:0e66ce2ab2fb
2?16?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 0:b1290ca6c4a2 1 //中央can program
YusukeWakuta 0:b1290ca6c4a2 2
YusukeWakuta 0:b1290ca6c4a2 3 #include "mbed.h"
taurin 2:9dc7d5f1e910 4 #define WAIT_LOOP_TIME 1
YusukeWakuta 0:b1290ca6c4a2 5 #define YOKUTAN_DATAS_NUM 5
YusukeWakuta 0:b1290ca6c4a2 6 #define IMPUT_DATAS_NUM 2
YusukeWakuta 0:b1290ca6c4a2 7 #define SEND_DATAS_CAN_ID 100
YusukeWakuta 0:b1290ca6c4a2 8
YusukeWakuta 0:b1290ca6c4a2 9 CAN can_R(p9,p10);
YusukeWakuta 0:b1290ca6c4a2 10 CAN can_L(p30,p29);
YusukeWakuta 0:b1290ca6c4a2 11 Serial toKeikiSerial(p28,p27);
YusukeWakuta 0:b1290ca6c4a2 12 Serial pc(USBTX,USBRX);
YusukeWakuta 1:65a3a2a4f535 13 //DigitalIn eruron_R(p7);
YusukeWakuta 1:65a3a2a4f535 14 //DigitalIn eruron_L(p7);
YusukeWakuta 1:65a3a2a4f535 15 int eruron_R = 0;
YusukeWakuta 1:65a3a2a4f535 16 int eruron_L = 0;
YusukeWakuta 0:b1290ca6c4a2 17 DigitalIn drug_R(p8);
YusukeWakuta 0:b1290ca6c4a2 18 DigitalIn drug_L(p8);
YusukeWakuta 0:b1290ca6c4a2 19 DigitalOut myled1(LED1);
YusukeWakuta 0:b1290ca6c4a2 20 DigitalOut myled2(LED2);
YusukeWakuta 0:b1290ca6c4a2 21
YusukeWakuta 0:b1290ca6c4a2 22 signed char yokutanDatas_R[YOKUTAN_DATAS_NUM];
YusukeWakuta 0:b1290ca6c4a2 23 signed char yokutanDatas_L[YOKUTAN_DATAS_NUM];
YusukeWakuta 0:b1290ca6c4a2 24 char inputDatas_R[IMPUT_DATAS_NUM];
YusukeWakuta 0:b1290ca6c4a2 25 char inputDatas_L[IMPUT_DATAS_NUM];
YusukeWakuta 0:b1290ca6c4a2 26
YusukeWakuta 0:b1290ca6c4a2 27 CANMessage recmsg_R(SEND_DATAS_CAN_ID, inputDatas_R, IMPUT_DATAS_NUM);
taurin 2:9dc7d5f1e910 28 CANMessage recmsg_L(SEND_DATAS_CAN_ID, inputDatas_L, IMPUT_DATAS_NUM);
YusukeWakuta 0:b1290ca6c4a2 29
YusukeWakuta 0:b1290ca6c4a2 30 void InputAndSentControlValues_R(){
taurin 2:9dc7d5f1e910 31 inputDatas_R[0] = 1 - inputDatas_R[0];
taurin 2:9dc7d5f1e910 32 inputDatas_R[1] = 1 - inputDatas_R[1];
taurin 2:9dc7d5f1e910 33 //for(int i = 0; i < IMPUT_DATAS_NUM; i++){
YusukeWakuta 0:b1290ca6c4a2 34 if(can_R.write(recmsg_R)){
taurin 2:9dc7d5f1e910 35 pc.printf("Right successed!\n\r");
YusukeWakuta 0:b1290ca6c4a2 36 }
taurin 2:9dc7d5f1e910 37 //}
YusukeWakuta 0:b1290ca6c4a2 38 }
YusukeWakuta 0:b1290ca6c4a2 39
YusukeWakuta 0:b1290ca6c4a2 40 void ledcheck_R() {
YusukeWakuta 0:b1290ca6c4a2 41 myled1 = 1;
YusukeWakuta 0:b1290ca6c4a2 42 wait(0.2);
YusukeWakuta 0:b1290ca6c4a2 43 myled1 = 0;
YusukeWakuta 0:b1290ca6c4a2 44 wait(0.2);
YusukeWakuta 0:b1290ca6c4a2 45 }
YusukeWakuta 0:b1290ca6c4a2 46
YusukeWakuta 0:b1290ca6c4a2 47 void ledcheck_L() {
YusukeWakuta 0:b1290ca6c4a2 48 myled2 = 1;
YusukeWakuta 0:b1290ca6c4a2 49 wait(0.2);
YusukeWakuta 0:b1290ca6c4a2 50 myled2 = 0;
YusukeWakuta 0:b1290ca6c4a2 51 wait(0.2);
YusukeWakuta 0:b1290ca6c4a2 52 }
YusukeWakuta 0:b1290ca6c4a2 53
YusukeWakuta 1:65a3a2a4f535 54 void servo_check(){
YusukeWakuta 1:65a3a2a4f535 55 int counter = 0;
YusukeWakuta 1:65a3a2a4f535 56 counter++;
YusukeWakuta 1:65a3a2a4f535 57 if(counter >= 170)
YusukeWakuta 1:65a3a2a4f535 58 counter = 0;
YusukeWakuta 1:65a3a2a4f535 59 if(counter >= 20){
YusukeWakuta 1:65a3a2a4f535 60 eruron_R = 1;
YusukeWakuta 1:65a3a2a4f535 61 eruron_L = 1;
YusukeWakuta 1:65a3a2a4f535 62 }
YusukeWakuta 1:65a3a2a4f535 63 else{
YusukeWakuta 1:65a3a2a4f535 64 eruron_R = 0;
YusukeWakuta 1:65a3a2a4f535 65 eruron_L = 0;
taurin 2:9dc7d5f1e910 66 }
taurin 2:9dc7d5f1e910 67 }
YusukeWakuta 1:65a3a2a4f535 68
YusukeWakuta 0:b1290ca6c4a2 69 void InputAndSentControlValues_L(){
taurin 2:9dc7d5f1e910 70 inputDatas_L[0] = 1 - inputDatas_L[0];
taurin 2:9dc7d5f1e910 71 inputDatas_L[1] = 1 - inputDatas_L[1];
taurin 2:9dc7d5f1e910 72 //for(int i = 0; i < IMPUT_DATAS_NUM; i++){
YusukeWakuta 0:b1290ca6c4a2 73 if(can_L.write(recmsg_L)){
taurin 2:9dc7d5f1e910 74 pc.printf("Left successed!\n\r");
YusukeWakuta 0:b1290ca6c4a2 75 }
taurin 2:9dc7d5f1e910 76 //}
YusukeWakuta 0:b1290ca6c4a2 77 }
YusukeWakuta 0:b1290ca6c4a2 78
YusukeWakuta 0:b1290ca6c4a2 79 void SendDarasToKeiki(){
YusukeWakuta 0:b1290ca6c4a2 80 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
YusukeWakuta 0:b1290ca6c4a2 81 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 82 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 83 }
YusukeWakuta 0:b1290ca6c4a2 84 }
YusukeWakuta 0:b1290ca6c4a2 85
YusukeWakuta 0:b1290ca6c4a2 86 void toString_R(){
YusukeWakuta 0:b1290ca6c4a2 87 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
YusukeWakuta 1:65a3a2a4f535 88 pc.printf("R : ");
YusukeWakuta 1:65a3a2a4f535 89 pc.printf("%i:",yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 90 }
taurin 2:9dc7d5f1e910 91 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
taurin 2:9dc7d5f1e910 92 pc.printf("%d:%d",i,inputDatas_R[i]);
taurin 2:9dc7d5f1e910 93 }
YusukeWakuta 0:b1290ca6c4a2 94 pc.printf("\t");
YusukeWakuta 0:b1290ca6c4a2 95 }
YusukeWakuta 0:b1290ca6c4a2 96
YusukeWakuta 0:b1290ca6c4a2 97 void toString_L(){
YusukeWakuta 0:b1290ca6c4a2 98 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
YusukeWakuta 1:65a3a2a4f535 99 pc.printf("L : ");
YusukeWakuta 1:65a3a2a4f535 100 pc.printf("%i:",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 101 }
YusukeWakuta 0:b1290ca6c4a2 102 pc.printf("\n\r");
YusukeWakuta 0:b1290ca6c4a2 103 }
YusukeWakuta 0:b1290ca6c4a2 104
YusukeWakuta 0:b1290ca6c4a2 105 void init(){
taurin 2:9dc7d5f1e910 106 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
taurin 2:9dc7d5f1e910 107 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 108 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 109 }
YusukeWakuta 0:b1290ca6c4a2 110 }
YusukeWakuta 0:b1290ca6c4a2 111
YusukeWakuta 0:b1290ca6c4a2 112
YusukeWakuta 0:b1290ca6c4a2 113 void receiveDatas(){
YusukeWakuta 0:b1290ca6c4a2 114 if(can_R.read(recmsg_R)){
YusukeWakuta 0:b1290ca6c4a2 115 ledcheck_R();
YusukeWakuta 0:b1290ca6c4a2 116 for(int i = 0; i < recmsg_R.len; i++){
YusukeWakuta 0:b1290ca6c4a2 117 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 0:b1290ca6c4a2 118 }
YusukeWakuta 0:b1290ca6c4a2 119 }
YusukeWakuta 0:b1290ca6c4a2 120 if(can_L.read(recmsg_L)){
YusukeWakuta 0:b1290ca6c4a2 121 ledcheck_L();
YusukeWakuta 0:b1290ca6c4a2 122 for(int i = 0; i < recmsg_L.len; i++){
YusukeWakuta 0:b1290ca6c4a2 123 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 0:b1290ca6c4a2 124 }
YusukeWakuta 0:b1290ca6c4a2 125 }
YusukeWakuta 0:b1290ca6c4a2 126 }
YusukeWakuta 0:b1290ca6c4a2 127
YusukeWakuta 0:b1290ca6c4a2 128
YusukeWakuta 0:b1290ca6c4a2 129 int main(){
YusukeWakuta 0:b1290ca6c4a2 130 init();
YusukeWakuta 0:b1290ca6c4a2 131 while(1){
taurin 2:9dc7d5f1e910 132 //servo_check();
YusukeWakuta 0:b1290ca6c4a2 133 InputAndSentControlValues_R();
YusukeWakuta 0:b1290ca6c4a2 134 InputAndSentControlValues_L();
YusukeWakuta 0:b1290ca6c4a2 135 wait_us(10);
YusukeWakuta 0:b1290ca6c4a2 136 receiveDatas();
YusukeWakuta 0:b1290ca6c4a2 137 toString_R();
YusukeWakuta 0:b1290ca6c4a2 138 toString_L();
YusukeWakuta 0:b1290ca6c4a2 139 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 140 }
YusukeWakuta 0:b1290ca6c4a2 141 }