Dependencies:   PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter

Fork of nhk_2018_undercarry_test08 by ケンタ ミヤザキ

Committer:
kenken0721
Date:
Thu Mar 22 06:16:03 2018 +0000
Revision:
1:f4dbd6b9cc27
Parent:
0:f6828b914b1c
Child:
2:c5996dd62e9c

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenken0721 0:f6828b914b1c 1 #include "mbed.h"
kenken0721 0:f6828b914b1c 2 #include "config.h"
kenken0721 0:f6828b914b1c 3 #include "moter.h"
kenken0721 0:f6828b914b1c 4 #include "PID.h"
kenken0721 1:f4dbd6b9cc27 5 #include "encoder.h"
kenken0721 0:f6828b914b1c 6
kenken0721 0:f6828b914b1c 7 Serial pc(USBTX, USBRX);
kenken0721 0:f6828b914b1c 8 Serial kbtpc(PC_12 ,PD_2);//serial5
kenken0721 0:f6828b914b1c 9 I2C master(sda, scl);
kenken0721 0:f6828b914b1c 10 DigitalOut led2(LED2);
kenken0721 0:f6828b914b1c 11 PID S_pid(SP, SI, SD, 0.0);
kenken0721 1:f4dbd6b9cc27 12 Encoder Enc_Carry_X(PA_15,PA_14,PC_1);
kenken0721 1:f4dbd6b9cc27 13 Encoder Enc_Carry_Y(PF_1,PA_1,PA_0);
kenken0721 1:f4dbd6b9cc27 14 int linecheck(char *buff);
kenken0721 0:f6828b914b1c 15
kenken0721 0:f6828b914b1c 16 PwmOut pwm_pins[] = {
kenken0721 0:f6828b914b1c 17 PwmOut( PC_8 ),
kenken0721 0:f6828b914b1c 18 PwmOut( PC_6 ),
kenken0721 0:f6828b914b1c 19 PwmOut( PB_14 ),
kenken0721 0:f6828b914b1c 20 PwmOut( PB_13 )
kenken0721 0:f6828b914b1c 21 };
kenken0721 0:f6828b914b1c 22
kenken0721 0:f6828b914b1c 23 DigitalOut dir_pins_1[] = {
kenken0721 0:f6828b914b1c 24 DigitalOut( PC_5 ),
kenken0721 0:f6828b914b1c 25 DigitalOut( PA_7 ),
kenken0721 0:f6828b914b1c 26 DigitalOut( PC_7 ),
kenken0721 0:f6828b914b1c 27 DigitalOut( PB_15)
kenken0721 0:f6828b914b1c 28 };
kenken0721 0:f6828b914b1c 29
kenken0721 0:f6828b914b1c 30 DigitalOut dir_pins_2[] = {
kenken0721 0:f6828b914b1c 31 DigitalOut( PB_12 ),
kenken0721 0:f6828b914b1c 32 DigitalOut( PA_6 ),
kenken0721 0:f6828b914b1c 33 DigitalOut( PB_6 ),
kenken0721 0:f6828b914b1c 34 DigitalOut( PB_1)
kenken0721 0:f6828b914b1c 35 };
kenken0721 0:f6828b914b1c 36
kenken0721 0:f6828b914b1c 37 bool mode = true;//trueでラジコン、falseでオート
kenken0721 0:f6828b914b1c 38 bool stopflag = true;//trueで機体停止
kenken0721 0:f6828b914b1c 39 int posit[2] = {0};//機体の現在位置
kenken0721 0:f6828b914b1c 40 int nextposit[2] = {0};//機体の位置指定
kenken0721 0:f6828b914b1c 41 double power = POWER;
kenken0721 0:f6828b914b1c 42 int kbtread = 0;
kenken0721 0:f6828b914b1c 43 int count = 0;
kenken0721 0:f6828b914b1c 44 int counter = 0;
kenken0721 0:f6828b914b1c 45 int linedata = 0;
kenken0721 0:f6828b914b1c 46 int prelinedata = 0;
kenken0721 0:f6828b914b1c 47 double output = 0.0;
kenken0721 1:f4dbd6b9cc27 48 char buff1[2];
kenken0721 1:f4dbd6b9cc27 49 char buff2[2];
kenken0721 1:f4dbd6b9cc27 50 char buff3[2];
kenken0721 1:f4dbd6b9cc27 51 float x_dist = 0.0;
kenken0721 1:f4dbd6b9cc27 52 float y_dist = 0.0;
kenken0721 1:f4dbd6b9cc27 53
kenken0721 1:f4dbd6b9cc27 54
kenken0721 1:f4dbd6b9cc27 55 int main() {
kenken0721 1:f4dbd6b9cc27 56
kenken0721 1:f4dbd6b9cc27 57 S_pid.init();
kenken0721 1:f4dbd6b9cc27 58 pwm_pins[0].period(0.00005);
kenken0721 1:f4dbd6b9cc27 59 pwm_pins[1].period(0.00005);
kenken0721 1:f4dbd6b9cc27 60 pwm_pins[2].period(0.00005);
kenken0721 1:f4dbd6b9cc27 61 pwm_pins[3].period(0.00005);
kenken0721 1:f4dbd6b9cc27 62 kbtpc.baud(2400);
kenken0721 1:f4dbd6b9cc27 63
kenken0721 1:f4dbd6b9cc27 64 char buff1[2];
kenken0721 1:f4dbd6b9cc27 65
kenken0721 1:f4dbd6b9cc27 66 while (true) {
kenken0721 1:f4dbd6b9cc27 67
kenken0721 1:f4dbd6b9cc27 68 master.read(addr1,buff1,2);
kenken0721 1:f4dbd6b9cc27 69 int line = linecheck(buff1);
kenken0721 1:f4dbd6b9cc27 70 kbtpc.putc(1);
kenken0721 1:f4dbd6b9cc27 71 if(kbtpc.readable()){
kenken0721 1:f4dbd6b9cc27 72 kbtread = kbtpc.getc();
kenken0721 1:f4dbd6b9cc27 73 }
kenken0721 1:f4dbd6b9cc27 74 if(kbtread == 255){
kenken0721 1:f4dbd6b9cc27 75 mode = true;
kenken0721 1:f4dbd6b9cc27 76 }else if(kbtread == 150){
kenken0721 1:f4dbd6b9cc27 77 mode = false;
kenken0721 1:f4dbd6b9cc27 78 }else if(kbtread == 100){
kenken0721 1:f4dbd6b9cc27 79 stopflag = true;
kenken0721 1:f4dbd6b9cc27 80 }else if(kbtread == 200){
kenken0721 1:f4dbd6b9cc27 81 stopflag = false;
kenken0721 1:f4dbd6b9cc27 82 }
kenken0721 1:f4dbd6b9cc27 83 //--------------手動--------------------------------------
kenken0721 1:f4dbd6b9cc27 84 if(mode == true){
kenken0721 1:f4dbd6b9cc27 85 led2 = 1.0;
kenken0721 1:f4dbd6b9cc27 86 if(kbtread == 1){
kenken0721 1:f4dbd6b9cc27 87 Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 88 }else if(kbtread == 2){
kenken0721 1:f4dbd6b9cc27 89 Back(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 90 }else if(kbtread == 3){
kenken0721 1:f4dbd6b9cc27 91 Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 92 }else if(kbtread == 4){
kenken0721 1:f4dbd6b9cc27 93 Left(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 94 }else{
kenken0721 1:f4dbd6b9cc27 95 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 96 }
kenken0721 1:f4dbd6b9cc27 97 //--------------自動---------------------------------
kenken0721 1:f4dbd6b9cc27 98 }else if(mode == false){
kenken0721 1:f4dbd6b9cc27 99 led2 = 0.0;
kenken0721 1:f4dbd6b9cc27 100 if(stopflag == true){
kenken0721 1:f4dbd6b9cc27 101 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 102 }else{
kenken0721 1:f4dbd6b9cc27 103 if(line == 255){
kenken0721 1:f4dbd6b9cc27 104 stopflag = true;
kenken0721 1:f4dbd6b9cc27 105 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 106 }else{
kenken0721 1:f4dbd6b9cc27 107 output = S_pid.compute((double)line);
kenken0721 1:f4dbd6b9cc27 108 if(output >= 0){
kenken0721 1:f4dbd6b9cc27 109 Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 110 }else {
kenken0721 1:f4dbd6b9cc27 111 Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 112 }
kenken0721 1:f4dbd6b9cc27 113 }
kenken0721 1:f4dbd6b9cc27 114 }
kenken0721 1:f4dbd6b9cc27 115 }
kenken0721 1:f4dbd6b9cc27 116 //pc.printf("kbtread = %d" ,kbtread);
kenken0721 1:f4dbd6b9cc27 117 ///pc.printf("buff[0] = %d" ,buff1[0]);
kenken0721 1:f4dbd6b9cc27 118 //pc.printf("buff[1] = %d" ,buff1[1]);
kenken0721 1:f4dbd6b9cc27 119 pc.printf("line = %d" ,line);
kenken0721 1:f4dbd6b9cc27 120 pc.printf("rpower = %d" ,rpower);
kenken0721 1:f4dbd6b9cc27 121 pc.printf("lpower = %d" ,lpower);
kenken0721 1:f4dbd6b9cc27 122 pc.printf("output = %lf \n" ,output);
kenken0721 1:f4dbd6b9cc27 123 }
kenken0721 1:f4dbd6b9cc27 124 }
kenken0721 1:f4dbd6b9cc27 125
kenken0721 0:f6828b914b1c 126
kenken0721 0:f6828b914b1c 127 int linecheck(char *buff){
kenken0721 0:f6828b914b1c 128 if(buff[0] == 24){
kenken0721 0:f6828b914b1c 129 linedata = 0;
kenken0721 0:f6828b914b1c 130 }else if(buff[0] == 56){
kenken0721 0:f6828b914b1c 131 linedata = 1;
kenken0721 0:f6828b914b1c 132 }else if(buff[0] == 48){
kenken0721 0:f6828b914b1c 133 linedata = 2;
kenken0721 0:f6828b914b1c 134 }else if(buff[0] == 112){
kenken0721 0:f6828b914b1c 135 linedata = 3;
kenken0721 0:f6828b914b1c 136 }else if(buff[0] == 96){
kenken0721 0:f6828b914b1c 137 linedata = 4;
kenken0721 0:f6828b914b1c 138 }else if(buff[0] == 224){
kenken0721 0:f6828b914b1c 139 linedata = 5;
kenken0721 0:f6828b914b1c 140 }else if(buff[0] == 192){
kenken0721 0:f6828b914b1c 141 linedata = 6;
kenken0721 0:f6828b914b1c 142 }else if(buff[0] == 128){
kenken0721 0:f6828b914b1c 143 linedata = 7;
kenken0721 0:f6828b914b1c 144 }else if(buff[0] == 28){
kenken0721 0:f6828b914b1c 145 linedata = -1;
kenken0721 0:f6828b914b1c 146 }else if(buff[0] == 12){
kenken0721 0:f6828b914b1c 147 linedata = -2;
kenken0721 0:f6828b914b1c 148 }else if(buff[0] == 14){
kenken0721 0:f6828b914b1c 149 linedata = -3;
kenken0721 0:f6828b914b1c 150 }else if(buff[0] == 6){
kenken0721 0:f6828b914b1c 151 linedata = -4;
kenken0721 0:f6828b914b1c 152 }else if(buff[0] == 7){
kenken0721 0:f6828b914b1c 153 linedata = -5;
kenken0721 0:f6828b914b1c 154 }else if(buff[0] == 3){
kenken0721 0:f6828b914b1c 155 linedata = -6;
kenken0721 0:f6828b914b1c 156 }else if(buff[0] == 1){
kenken0721 0:f6828b914b1c 157 linedata = -7;
kenken0721 0:f6828b914b1c 158 }else if(buff[0] == 15 && buff[1] == 15){
kenken0721 0:f6828b914b1c 159 linedata = 255;
kenken0721 0:f6828b914b1c 160 }else if(buff[0] == 0 && buff[1] == 0){
kenken0721 1:f4dbd6b9cc27 161 linedata = prelinedata;
kenken0721 0:f6828b914b1c 162 }else{
kenken0721 0:f6828b914b1c 163 linedata = prelinedata;
kenken0721 0:f6828b914b1c 164 }
kenken0721 0:f6828b914b1c 165 prelinedata = linedata;
kenken0721 0:f6828b914b1c 166
kenken0721 0:f6828b914b1c 167 return linedata;
kenken0721 1:f4dbd6b9cc27 168 }