albatross / Mbed 2 deprecated Control_Main_Full_20160608

6/8 操舵中央 ピッチ操作追加版

Dependencies:   mbed

Fork of Souda_Main_ver3 by albatross

main.cpp@1:65a3a2a4f535, 2016-02-16 (annotated)

Committer:
YusukeWakuta
Date:
Tue Feb 16 08:09:38 2016 +0000
Revision:
1:65a3a2a4f535
Parent:
0:b1290ca6c4a2
Child:
2:9dc7d5f1e910
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"
YusukeWakuta 0:b1290ca6c4a2 4 #define WAIT_LOOP_TIME 0.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);
YusukeWakuta 0:b1290ca6c4a2 28 CANMessage recmsg_L(SEND_DATAS_CAN_ID, inputDatas_R, IMPUT_DATAS_NUM);
YusukeWakuta 0:b1290ca6c4a2 29
YusukeWakuta 0:b1290ca6c4a2 30 void InputAndSentControlValues_R(){
YusukeWakuta 0:b1290ca6c4a2 31 inputDatas_R[0] = eruron_R;
YusukeWakuta 0:b1290ca6c4a2 32 inputDatas_R[1] = drug_R;
YusukeWakuta 0:b1290ca6c4a2 33 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
YusukeWakuta 0:b1290ca6c4a2 34 if(can_R.write(recmsg_R)){
YusukeWakuta 1:65a3a2a4f535 35 //pc.printf("Right successed!\n\r");
YusukeWakuta 0:b1290ca6c4a2 36 }
YusukeWakuta 0:b1290ca6c4a2 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;
YusukeWakuta 1:65a3a2a4f535 66 }
YusukeWakuta 1:65a3a2a4f535 67 }
YusukeWakuta 1:65a3a2a4f535 68
YusukeWakuta 0:b1290ca6c4a2 69 void InputAndSentControlValues_L(){
YusukeWakuta 0:b1290ca6c4a2 70 inputDatas_L[0] = eruron_L;
YusukeWakuta 0:b1290ca6c4a2 71 inputDatas_L[1] = drug_L;
YusukeWakuta 0:b1290ca6c4a2 72 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
YusukeWakuta 0:b1290ca6c4a2 73 if(can_L.write(recmsg_L)){
YusukeWakuta 1:65a3a2a4f535 74 // pc.printf("Left successed!\n\r");
YusukeWakuta 0:b1290ca6c4a2 75 }
YusukeWakuta 0:b1290ca6c4a2 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 }
YusukeWakuta 0:b1290ca6c4a2 91 pc.printf("\t");
YusukeWakuta 0:b1290ca6c4a2 92 }
YusukeWakuta 0:b1290ca6c4a2 93
YusukeWakuta 0:b1290ca6c4a2 94 void toString_L(){
YusukeWakuta 0:b1290ca6c4a2 95 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
YusukeWakuta 1:65a3a2a4f535 96 pc.printf("L : ");
YusukeWakuta 1:65a3a2a4f535 97 pc.printf("%i:",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 98 }
YusukeWakuta 0:b1290ca6c4a2 99 pc.printf("\n\r");
YusukeWakuta 0:b1290ca6c4a2 100 }
YusukeWakuta 0:b1290ca6c4a2 101
YusukeWakuta 0:b1290ca6c4a2 102 void init(){
YusukeWakuta 0:b1290ca6c4a2 103 }
YusukeWakuta 0:b1290ca6c4a2 104
YusukeWakuta 0:b1290ca6c4a2 105
YusukeWakuta 0:b1290ca6c4a2 106 void receiveDatas(){
YusukeWakuta 0:b1290ca6c4a2 107 if(can_R.read(recmsg_R)){
YusukeWakuta 0:b1290ca6c4a2 108 ledcheck_R();
YusukeWakuta 0:b1290ca6c4a2 109 for(int i = 0; i < recmsg_R.len; i++){
YusukeWakuta 0:b1290ca6c4a2 110 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 0:b1290ca6c4a2 111 }
YusukeWakuta 0:b1290ca6c4a2 112 }
YusukeWakuta 0:b1290ca6c4a2 113 if(can_L.read(recmsg_L)){
YusukeWakuta 0:b1290ca6c4a2 114 ledcheck_L();
YusukeWakuta 0:b1290ca6c4a2 115 for(int i = 0; i < recmsg_L.len; i++){
YusukeWakuta 0:b1290ca6c4a2 116 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 0:b1290ca6c4a2 117 }
YusukeWakuta 0:b1290ca6c4a2 118 }
YusukeWakuta 0:b1290ca6c4a2 119 }
YusukeWakuta 0:b1290ca6c4a2 120
YusukeWakuta 0:b1290ca6c4a2 121
YusukeWakuta 0:b1290ca6c4a2 122 int main(){
YusukeWakuta 0:b1290ca6c4a2 123 init();
YusukeWakuta 0:b1290ca6c4a2 124 while(1){
YusukeWakuta 1:65a3a2a4f535 125 servo_check();
YusukeWakuta 0:b1290ca6c4a2 126 InputAndSentControlValues_R();
YusukeWakuta 0:b1290ca6c4a2 127 InputAndSentControlValues_L();
YusukeWakuta 0:b1290ca6c4a2 128 wait_us(10);
YusukeWakuta 0:b1290ca6c4a2 129 receiveDatas();
YusukeWakuta 0:b1290ca6c4a2 130 toString_R();
YusukeWakuta 0:b1290ca6c4a2 131 toString_L();
YusukeWakuta 0:b1290ca6c4a2 132 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 133 }
YusukeWakuta 0:b1290ca6c4a2 134 }