ケンタ ミヤザキ / Mbed 2 deprecated nhk_2018_undercarry_test09

Dependencies:   PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter

Fork of nhk_2018_undercarry_test08 by ケンタ ミヤザキ

main.cpp@0:f6828b914b1c, 2018-03-21 (annotated)

Committer:
kenken0721
Date:
Wed Mar 21 06:51:59 2018 +0000
Revision:
0:f6828b914b1c
Child:
1:f4dbd6b9cc27
?

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