yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

main.cpp@0:411ab20ce87d, 2019-04-26 (annotated)

Committer:
eri
Date:
Fri Apr 26 11:34:44 2019 +0000
Revision:
0:411ab20ce87d
Child:
1:34371ffd3dc0
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:411ab20ce87d 1 #include "mbed.h"
eri 0:411ab20ce87d 2 #include "pin.h"
eri 0:411ab20ce87d 3
eri 0:411ab20ce87d 4
eri 0:411ab20ce87d 5
eri 0:411ab20ce87d 6 ////////////関数
eri 0:411ab20ce87d 7 void setup();
eri 0:411ab20ce87d 8 void can_receive();
eri 0:411ab20ce87d 9 void pid(double,double);
eri 0:411ab20ce87d 10 void out_ro(double);
eri 0:411ab20ce87d 11 void out_ri(double);
eri 0:411ab20ce87d 12
eri 0:411ab20ce87d 13
eri 0:411ab20ce87d 14 ////////////定数
eri 0:411ab20ce87d 15 int solution=500;
eri 0:411ab20ce87d 16 int c_degree=360/500; //solution=500
eri 0:411ab20ce87d 17
eri 0:411ab20ce87d 18 double Kp_ro=0.01;
eri 0:411ab20ce87d 19 double Ki_ro=0;
eri 0:411ab20ce87d 20 double Kd_ro=0;
eri 0:411ab20ce87d 21 double Kp_ri=0.01;
eri 0:411ab20ce87d 22 double Ki_ri=0;
eri 0:411ab20ce87d 23 double Kd_ri=0;
eri 0:411ab20ce87d 24
eri 0:411ab20ce87d 25
eri 0:411ab20ce87d 26
eri 0:411ab20ce87d 27
eri 0:411ab20ce87d 28 ////////////変数
eri 0:411ab20ce87d 29 double target_ro=0,target_ri=0;
eri 0:411ab20ce87d 30 bool hand_mode=0;
eri 0:411ab20ce87d 31 double distance_ro_old=0,distance_ri_old=0,pile_ro=0,pile_ri=0;
eri 0:411ab20ce87d 32 double posi_ro=0,posi_ri=0;
eri 0:411ab20ce87d 33 double pre_time=0;
eri 0:411ab20ce87d 34
eri 0:411ab20ce87d 35
eri 0:411ab20ce87d 36 Timer timer;
eri 0:411ab20ce87d 37
eri 0:411ab20ce87d 38
eri 0:411ab20ce87d 39
eri 0:411ab20ce87d 40
eri 0:411ab20ce87d 41 //////////////////////////////////////////////
eri 0:411ab20ce87d 42 int main() {
eri 0:411ab20ce87d 43
eri 0:411ab20ce87d 44 setup();
eri 0:411ab20ce87d 45 while(1) {
eri 0:411ab20ce87d 46 pid(target_ro,target_ri);
eri 0:411ab20ce87d 47 wait(0.01);
eri 0:411ab20ce87d 48 }
eri 0:411ab20ce87d 49 }
eri 0:411ab20ce87d 50
eri 0:411ab20ce87d 51 //////////////////////////////////////////////
eri 0:411ab20ce87d 52 void setup(){
eri 0:411ab20ce87d 53 can.frequency(1000000);
eri 0:411ab20ce87d 54 motor_ro_f.period_us(100);
eri 0:411ab20ce87d 55 motor_ro_b.period_us(100);
eri 0:411ab20ce87d 56 motor_ri_f.period_us(100);
eri 0:411ab20ce87d 57 motor_ri_b.period_us(100);
eri 0:411ab20ce87d 58
eri 0:411ab20ce87d 59 switch1.mode(PullUp);
eri 0:411ab20ce87d 60 switch2.mode(PullUp);
eri 0:411ab20ce87d 61
eri 0:411ab20ce87d 62 servo.init();
eri 0:411ab20ce87d 63 timer.start();
eri 0:411ab20ce87d 64 }
eri 0:411ab20ce87d 65
eri 0:411ab20ce87d 66
eri 0:411ab20ce87d 67 void pid(double target_ro_,double target_ri_){
eri 0:411ab20ce87d 68 posi_ro=(ec_ro.getCount()%solution)*c_degree;
eri 0:411ab20ce87d 69 posi_ri=(ec_ri.getCount()%solution)*c_degree;
eri 0:411ab20ce87d 70
eri 0:411ab20ce87d 71 double now=timer.read();
eri 0:411ab20ce87d 72 double d_time=now-pre_time;
eri 0:411ab20ce87d 73
eri 0:411ab20ce87d 74 double deviation_ro=fabs(target_ro_)-posi_ro;
eri 0:411ab20ce87d 75 if(target_ro_>0){
eri 0:411ab20ce87d 76 if(deviation_ro<0)deviation_ro=360+deviation_ro;
eri 0:411ab20ce87d 77 }else{
eri 0:411ab20ce87d 78 if(deviation_ro>0)deviation_ro=deviation_ro-360;
eri 0:411ab20ce87d 79 }
eri 0:411ab20ce87d 80 double deviation_ri=fabs(target_ri_)-posi_ri;
eri 0:411ab20ce87d 81 if(target_ri_>0){
eri 0:411ab20ce87d 82 if(deviation_ri<0)deviation_ri=360+deviation_ri;
eri 0:411ab20ce87d 83 }else{
eri 0:411ab20ce87d 84 if(deviation_ri>0)deviation_ri=deviation_ri-360;
eri 0:411ab20ce87d 85 }
eri 0:411ab20ce87d 86
eri 0:411ab20ce87d 87 pile_ro+=deviation_ro;
eri 0:411ab20ce87d 88 pile_ri+=deviation_ri;
eri 0:411ab20ce87d 89
eri 0:411ab20ce87d 90 out_ro(deviation_ro * Kp_ro + (posi_ro - distance_ro_old) / d_time * Kd_ro + pile_ro * Ki_ro * d_time);
eri 0:411ab20ce87d 91 out_ri(deviation_ri * Kp_ri + (posi_ri - distance_ri_old) / d_time * Kd_ri + pile_ri * Ki_ri * d_time);
eri 0:411ab20ce87d 92
eri 0:411ab20ce87d 93 distance_ro_old=deviation_ro;
eri 0:411ab20ce87d 94 distance_ri_old=deviation_ri;
eri 0:411ab20ce87d 95 pre_time=now;
eri 0:411ab20ce87d 96 }
eri 0:411ab20ce87d 97
eri 0:411ab20ce87d 98
eri 0:411ab20ce87d 99 void out_ro(double duty){
eri 0:411ab20ce87d 100 double dutylimit=0.4;
eri 0:411ab20ce87d 101
eri 0:411ab20ce87d 102 if(duty > 0) { //入力duty比が正の場合
eri 0:411ab20ce87d 103 //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
eri 0:411ab20ce87d 104 if( fabs( duty ) < dutylimit ) { //制限値内
eri 0:411ab20ce87d 105 motor_ro_f = fabs(duty);
eri 0:411ab20ce87d 106 motor_ro_b = 0;
eri 0:411ab20ce87d 107 } else { //制限値超
eri 0:411ab20ce87d 108 motor_ro_f = dutylimit;
eri 0:411ab20ce87d 109 motor_ro_b = 0;
eri 0:411ab20ce87d 110 }
eri 0:411ab20ce87d 111 } else {//入力duty比が負の場合
eri 0:411ab20ce87d 112 //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max;
eri 0:411ab20ce87d 113 if( fabs(duty) < dutylimit) { //制限値内
eri 0:411ab20ce87d 114 motor_ro_f = 0;
eri 0:411ab20ce87d 115 motor_ro_b = fabs(duty);
eri 0:411ab20ce87d 116 } else { //制限値超
eri 0:411ab20ce87d 117 motor_ro_f = 0;
eri 0:411ab20ce87d 118 motor_ro_b = dutylimit;
eri 0:411ab20ce87d 119 }
eri 0:411ab20ce87d 120 }
eri 0:411ab20ce87d 121 //pre_out_r=duty;
eri 0:411ab20ce87d 122 }
eri 0:411ab20ce87d 123
eri 0:411ab20ce87d 124
eri 0:411ab20ce87d 125 void out_ri(double duty){
eri 0:411ab20ce87d 126 double dutylimit=0.4;
eri 0:411ab20ce87d 127
eri 0:411ab20ce87d 128 if(duty > 0) { //入力duty比が正の場合
eri 0:411ab20ce87d 129 //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
eri 0:411ab20ce87d 130 if( fabs( duty ) < dutylimit ) { //制限値内
eri 0:411ab20ce87d 131 motor_ri_f = fabs(duty);
eri 0:411ab20ce87d 132 motor_ri_b = 0;
eri 0:411ab20ce87d 133 } else { //制限値超
eri 0:411ab20ce87d 134 motor_ri_f = dutylimit;
eri 0:411ab20ce87d 135 motor_ri_b = 0;
eri 0:411ab20ce87d 136 }
eri 0:411ab20ce87d 137 } else {//入力duty比が負の場合
eri 0:411ab20ce87d 138 //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max;
eri 0:411ab20ce87d 139 if( fabs(duty) < dutylimit) { //制限値内
eri 0:411ab20ce87d 140 motor_ri_f = 0;
eri 0:411ab20ce87d 141 motor_ri_b = fabs(duty);
eri 0:411ab20ce87d 142 } else { //制限値超
eri 0:411ab20ce87d 143 motor_ri_f = 0;
eri 0:411ab20ce87d 144 motor_ri_b = dutylimit;
eri 0:411ab20ce87d 145 }
eri 0:411ab20ce87d 146 }
eri 0:411ab20ce87d 147 //pre_out_r=duty;
eri 0:411ab20ce87d 148 }
eri 0:411ab20ce87d 149
eri 0:411ab20ce87d 150 //////////////////////////////////////////can
eri 0:411ab20ce87d 151 void can_receive(){
eri 0:411ab20ce87d 152 CANMessage msg;
eri 0:411ab20ce87d 153 for(int i=0; i<5; i++) {
eri 0:411ab20ce87d 154 if(can.read(msg)) {
eri 0:411ab20ce87d 155 if(msg.id==0) {
eri 0:411ab20ce87d 156 target_ro= msg.data[0] | ((msg.data[1]&0b1111)<<8) - 360;
eri 0:411ab20ce87d 157 target_ri= msg.data[2] | ((msg.data[1]&0b11110000)<<4) - 360;
eri 0:411ab20ce87d 158 hand_mode= msg.data[3];
eri 0:411ab20ce87d 159
eri 0:411ab20ce87d 160 break;
eri 0:411ab20ce87d 161 }
eri 0:411ab20ce87d 162 led2=1;
eri 0:411ab20ce87d 163 } else led2=0;
eri 0:411ab20ce87d 164 }
eri 0:411ab20ce87d 165 }