albatross / Mbed 2 deprecated Souda_Main_ver3

3/18 操舵中央

Dependencies:   mbed

Fork of Control_Main_Full_ver2 by albatross

main.cpp@5:6b74fa61198e, 2016-02-25 (annotated)

Committer:
taurin
Date:
Thu Feb 25 09:13:08 2016 +0000
Revision:
5:6b74fa61198e
Parent:
4:4a13dd263a7b
Child:
6:7484315ba88a
aaaa;

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 4:4a13dd263a7b 4 #define WAIT_LOOP_TIME 0.1
taurin 4:4a13dd263a7b 5 #define YOKUTAN_DATAS_NUM 4
YusukeWakuta 0:b1290ca6c4a2 6 #define IMPUT_DATAS_NUM 2
YusukeWakuta 0:b1290ca6c4a2 7 #define SEND_DATAS_CAN_ID 100
taurin 4:4a13dd263a7b 8 #define SEND_DATAS_TIME 0.5
taurin 4:4a13dd263a7b 9 #define THRESHOLD_OH_VALUE 0.14
taurin 4:4a13dd263a7b 10 #define CALC_CADENCE_TIME 0.05
YusukeWakuta 0:b1290ca6c4a2 11
YusukeWakuta 0:b1290ca6c4a2 12 CAN can_R(p9,p10);
YusukeWakuta 0:b1290ca6c4a2 13 CAN can_L(p30,p29);
taurin 4:4a13dd263a7b 14 Serial toKeikiSerial(p13,p14);
YusukeWakuta 0:b1290ca6c4a2 15 Serial pc(USBTX,USBRX);
taurin 5:6b74fa61198e 16 DigitalIn eruron_R(p23);
taurin 5:6b74fa61198e 17 DigitalIn eruron_L(p19);
taurin 5:6b74fa61198e 18 DigitalIn drug_R(p22);
taurin 5:6b74fa61198e 19 DigitalIn drug_L(p20);
YusukeWakuta 0:b1290ca6c4a2 20 DigitalOut myled1(LED1);
YusukeWakuta 0:b1290ca6c4a2 21 DigitalOut myled2(LED2);
taurin 4:4a13dd263a7b 22 Ticker sendDatasTicker;
taurin 4:4a13dd263a7b 23 Ticker cadenceTicker;
taurin 4:4a13dd263a7b 24 Timer cadenceTimer;
taurin 4:4a13dd263a7b 25 AnalogIn p(p15);
taurin 4:4a13dd263a7b 26 AnalogIn q(p16);
YusukeWakuta 0:b1290ca6c4a2 27
taurin 4:4a13dd263a7b 28 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 29 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
YusukeWakuta 0:b1290ca6c4a2 30 char inputDatas_R[IMPUT_DATAS_NUM];
YusukeWakuta 0:b1290ca6c4a2 31 char inputDatas_L[IMPUT_DATAS_NUM];
taurin 4:4a13dd263a7b 32 char cadence = 0;
taurin 4:4a13dd263a7b 33 bool stateP = true;
taurin 4:4a13dd263a7b 34 bool stateQ = true;
taurin 4:4a13dd263a7b 35 bool stateP_old = true;
taurin 4:4a13dd263a7b 36 bool stateQ_old = true;
YusukeWakuta 0:b1290ca6c4a2 37
YusukeWakuta 3:0e66ce2ab2fb 38 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 39 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 40
taurin 4:4a13dd263a7b 41 void InputControlValues(){
taurin 4:4a13dd263a7b 42 inputDatas_R[1] = (char)drug_R;
taurin 4:4a13dd263a7b 43 inputDatas_L[1] = (char)drug_L;
taurin 4:4a13dd263a7b 44 if(eruron_R){
taurin 4:4a13dd263a7b 45 inputDatas_R[0] = 0;
taurin 4:4a13dd263a7b 46 inputDatas_L[0] = 2;
taurin 4:4a13dd263a7b 47 }
taurin 4:4a13dd263a7b 48 else if(eruron_L){
taurin 4:4a13dd263a7b 49 inputDatas_R[0] = 2;
taurin 4:4a13dd263a7b 50 inputDatas_L[0] = 0;
taurin 4:4a13dd263a7b 51 }
taurin 4:4a13dd263a7b 52 else{
taurin 4:4a13dd263a7b 53 inputDatas_R[0] = 1;
taurin 4:4a13dd263a7b 54 inputDatas_L[0] = 1;
taurin 4:4a13dd263a7b 55 }
YusukeWakuta 0:b1290ca6c4a2 56 }
YusukeWakuta 0:b1290ca6c4a2 57
taurin 4:4a13dd263a7b 58 void toString_R(){
taurin 4:4a13dd263a7b 59 pc.printf("R:");
taurin 4:4a13dd263a7b 60 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
taurin 4:4a13dd263a7b 61 pc.printf("%d:%i ",i,inputDatas_R[i]);
taurin 4:4a13dd263a7b 62 }
YusukeWakuta 0:b1290ca6c4a2 63 }
YusukeWakuta 0:b1290ca6c4a2 64
taurin 4:4a13dd263a7b 65 void toString_L(){
taurin 4:4a13dd263a7b 66 pc.printf("L:");
taurin 4:4a13dd263a7b 67 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
taurin 4:4a13dd263a7b 68 pc.printf("%d:%i ",i,inputDatas_L[i]);
taurin 4:4a13dd263a7b 69 }
taurin 4:4a13dd263a7b 70 pc.printf("\n\r");
YusukeWakuta 0:b1290ca6c4a2 71 }
YusukeWakuta 0:b1290ca6c4a2 72
taurin 4:4a13dd263a7b 73 void SendDatas(){
taurin 4:4a13dd263a7b 74 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, IMPUT_DATAS_NUM));
taurin 4:4a13dd263a7b 75 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, IMPUT_DATAS_NUM));
YusukeWakuta 0:b1290ca6c4a2 76 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
YusukeWakuta 0:b1290ca6c4a2 77 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 78 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 79 }
taurin 4:4a13dd263a7b 80 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
taurin 4:4a13dd263a7b 81 toKeikiSerial.putc(inputDatas_R[i]);
taurin 4:4a13dd263a7b 82 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 83 }
taurin 4:4a13dd263a7b 84 toKeikiSerial.putc(cadence);
taurin 4:4a13dd263a7b 85 pc.printf("test\n\r");
YusukeWakuta 0:b1290ca6c4a2 86 }
YusukeWakuta 0:b1290ca6c4a2 87
taurin 4:4a13dd263a7b 88 void calcCadence(){
taurin 4:4a13dd263a7b 89 //pc.printf("%f,%f\n\r",p.read(),q.read());
taurin 4:4a13dd263a7b 90 if(p.read() > THRESHOLD_OH_VALUE) stateP = true;
taurin 4:4a13dd263a7b 91 else stateP = false;
taurin 4:4a13dd263a7b 92 if(q.read() > THRESHOLD_OH_VALUE) stateQ = true;
taurin 4:4a13dd263a7b 93 else stateQ = false;
taurin 4:4a13dd263a7b 94 if((stateP == true && stateP_old == false) ||
taurin 4:4a13dd263a7b 95 stateQ == true && stateQ_old == false){
taurin 4:4a13dd263a7b 96 cadenceTimer.stop();
taurin 4:4a13dd263a7b 97 cadence = (char)30.0/cadenceTimer.read();
taurin 4:4a13dd263a7b 98 pc.printf("Cadence:%i\n\r",cadence);
taurin 4:4a13dd263a7b 99 cadenceTimer.reset();
taurin 4:4a13dd263a7b 100 cadenceTimer.start();
taurin 2:9dc7d5f1e910 101 }
taurin 4:4a13dd263a7b 102 stateP_old = stateP;
taurin 4:4a13dd263a7b 103 stateQ_old = stateQ;
taurin 4:4a13dd263a7b 104 }
YusukeWakuta 0:b1290ca6c4a2 105
YusukeWakuta 0:b1290ca6c4a2 106 void init(){
taurin 4:4a13dd263a7b 107 cadenceTimer.start();
taurin 4:4a13dd263a7b 108 cadenceTicker.attach(calcCadence, CALC_CADENCE_TIME);
taurin 4:4a13dd263a7b 109 sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
taurin 2:9dc7d5f1e910 110 for(int i = 0; i < IMPUT_DATAS_NUM; i++){
taurin 2:9dc7d5f1e910 111 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 112 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 113 }
YusukeWakuta 0:b1290ca6c4a2 114 }
YusukeWakuta 0:b1290ca6c4a2 115
YusukeWakuta 0:b1290ca6c4a2 116
YusukeWakuta 0:b1290ca6c4a2 117 void receiveDatas(){
YusukeWakuta 0:b1290ca6c4a2 118 if(can_R.read(recmsg_R)){
YusukeWakuta 0:b1290ca6c4a2 119 for(int i = 0; i < recmsg_R.len; i++){
YusukeWakuta 0:b1290ca6c4a2 120 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 0:b1290ca6c4a2 121 }
taurin 4:4a13dd263a7b 122 myled1 = !myled1;
YusukeWakuta 0:b1290ca6c4a2 123 }
YusukeWakuta 0:b1290ca6c4a2 124 if(can_L.read(recmsg_L)){
YusukeWakuta 0:b1290ca6c4a2 125 for(int i = 0; i < recmsg_L.len; i++){
YusukeWakuta 0:b1290ca6c4a2 126 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 0:b1290ca6c4a2 127 }
taurin 4:4a13dd263a7b 128 myled2 = !myled2;
YusukeWakuta 0:b1290ca6c4a2 129 }
YusukeWakuta 0:b1290ca6c4a2 130 }
YusukeWakuta 0:b1290ca6c4a2 131
YusukeWakuta 0:b1290ca6c4a2 132 int main(){
YusukeWakuta 0:b1290ca6c4a2 133 init();
YusukeWakuta 0:b1290ca6c4a2 134 while(1){
taurin 4:4a13dd263a7b 135 InputControlValues();
YusukeWakuta 0:b1290ca6c4a2 136 wait_us(10);
YusukeWakuta 0:b1290ca6c4a2 137 receiveDatas();
taurin 4:4a13dd263a7b 138 //toString_R();
taurin 4:4a13dd263a7b 139 // toString_L();
YusukeWakuta 0:b1290ca6c4a2 140 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 141 }
YusukeWakuta 0:b1290ca6c4a2 142 }