新部内対抗A班 / Mbed 2 deprecated circle_war_ver_A_NUCLEO__

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of circle_war_ver_A_NUCLEO_ by 新部内対抗A班

speed_control.cpp@9:92ddadd9e9a9, 2016-04-04 (annotated)

Committer:
username16248
Date:
Mon Apr 04 07:20:24 2016 +0000
Revision:
9:92ddadd9e9a9
Child:
11:f8fac9693cd5
???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
username16248 9:92ddadd9e9a9 1 #include "mbed.h"
username16248 9:92ddadd9e9a9 2 #include <pin_file.h>
username16248 9:92ddadd9e9a9 3 #include <speed_control.h>
username16248 9:92ddadd9e9a9 4 #include <encoder.h>
username16248 9:92ddadd9e9a9 5 #define DUTY_MAX 0.9f
username16248 9:92ddadd9e9a9 6
username16248 9:92ddadd9e9a9 7 PwmOut wheelf1(WHf1);//前部
username16248 9:92ddadd9e9a9 8 PwmOut wheelf2(WHf2);
username16248 9:92ddadd9e9a9 9 PwmOut wheelr1(WHr1);//右部
username16248 9:92ddadd9e9a9 10 PwmOut wheelr2(WHr2);
username16248 9:92ddadd9e9a9 11 PwmOut wheell1(WHl1);//左部
username16248 9:92ddadd9e9a9 12 PwmOut wheell2(WHl2);
username16248 9:92ddadd9e9a9 13 Ticker rot_cal;
username16248 9:92ddadd9e9a9 14 float angle_f=0.0,angle_r=0.0,angle_l=0.0;
username16248 9:92ddadd9e9a9 15 float angle_old_f=0.0,angle_old_r=0.0,angle_old_l=0.0;
username16248 9:92ddadd9e9a9 16 float duty_out_f=0.0,duty_out_r=0.0,duty_out_l=0.0;
username16248 9:92ddadd9e9a9 17 float duty_PID_f=0.0,duty_PID_r=0.0,duty_PID_l=0.0;
username16248 9:92ddadd9e9a9 18 float omega_now_f=0.0,omega_now_r=0.0,omega_now_l=0.0;
username16248 9:92ddadd9e9a9 19 float rot_now_f=0.0,rot_now_r=0.0,rot_now_l=0.0;
username16248 9:92ddadd9e9a9 20 float rot_old_f=0.0,rot_old_r=0.0,rot_old_l=0.0;
username16248 9:92ddadd9e9a9 21 float rot_PID_f=0.0,rot_PID_r=0.0,rot_PID_l=0.0;
username16248 9:92ddadd9e9a9 22 float rot_prop_f=0.0,rot_prop_r=0.0,rot_prop_l=0.0;
username16248 9:92ddadd9e9a9 23 float rot_diff_f=0.0,rot_diff_r=0.0,rot_diff_l=0.0;
username16248 9:92ddadd9e9a9 24 float rot_inte_f=0.0,rot_inte_r=0.0,rot_inte_l=0.0;
username16248 9:92ddadd9e9a9 25 int dir_f,dir_r,dir_l;
username16248 9:92ddadd9e9a9 26 const float Kp_f=30.0000f,Kd_f=200.0000f,Ki_f=0.0001f;
username16248 9:92ddadd9e9a9 27 const float Kp_r=30.0000f,Kd_r=200.0000f,Ki_r=0.0001f;
username16248 9:92ddadd9e9a9 28 const float Kp_l=30.0000f,Kd_l=200.0000f,Ki_l=0.0001f;
username16248 9:92ddadd9e9a9 29 const float resolution=1024.0;
username16248 9:92ddadd9e9a9 30 int t=0;
username16248 9:92ddadd9e9a9 31 /*
username16248 9:92ddadd9e9a9 32 void motor_move()
username16248 9:92ddadd9e9a9 33 {
username16248 9:92ddadd9e9a9 34 wheelf1=0.1;
username16248 9:92ddadd9e9a9 35 wheelf2=0;
username16248 9:92ddadd9e9a9 36 wheelr1=0.1;
username16248 9:92ddadd9e9a9 37 wheelr2=0;
username16248 9:92ddadd9e9a9 38 wheell1=0.1;
username16248 9:92ddadd9e9a9 39 wheell2=0;
username16248 9:92ddadd9e9a9 40 }
username16248 9:92ddadd9e9a9 41 */
username16248 9:92ddadd9e9a9 42
username16248 9:92ddadd9e9a9 43
username16248 9:92ddadd9e9a9 44 ////////////////////////////モーターそれぞれのPID制御/////////////////////////
username16248 9:92ddadd9e9a9 45 void motor_f(int dir_f,float rot_target_f)
username16248 9:92ddadd9e9a9 46 {
username16248 9:92ddadd9e9a9 47 rot_prop_f=rot_target_f-rot_now_f;
username16248 9:92ddadd9e9a9 48 //rot_diff_f=rot_now_f-rot_old_f;
username16248 9:92ddadd9e9a9 49 rot_inte_f+=rot_prop_f;
username16248 9:92ddadd9e9a9 50 rot_PID_f=Kp_f*rot_prop_f+Kd_f*rot_diff_f;//+Ki_f*rot_inte;
username16248 9:92ddadd9e9a9 51 duty_PID_f=rot_PID_f*1.092f*0.0001f;
username16248 9:92ddadd9e9a9 52 duty_out_f=duty_out_f+duty_PID_f;
username16248 9:92ddadd9e9a9 53
username16248 9:92ddadd9e9a9 54 if(duty_out_f>DUTY_MAX) {
username16248 9:92ddadd9e9a9 55 duty_out_f=DUTY_MAX;
username16248 9:92ddadd9e9a9 56 } else if(duty_out_f<0) {
username16248 9:92ddadd9e9a9 57 duty_out_f=0;
username16248 9:92ddadd9e9a9 58 }
username16248 9:92ddadd9e9a9 59
username16248 9:92ddadd9e9a9 60 if(dir_f==1) {
username16248 9:92ddadd9e9a9 61 wheelf1=duty_out_f;
username16248 9:92ddadd9e9a9 62 wheelf2=0;
username16248 9:92ddadd9e9a9 63 } else if(dir_f==-1) {
username16248 9:92ddadd9e9a9 64 wheelf1=0;
username16248 9:92ddadd9e9a9 65 wheelf2=duty_out_f;
username16248 9:92ddadd9e9a9 66 } else {
username16248 9:92ddadd9e9a9 67 wheelf1=0;
username16248 9:92ddadd9e9a9 68 wheelf2=0;
username16248 9:92ddadd9e9a9 69 }
username16248 9:92ddadd9e9a9 70 }
username16248 9:92ddadd9e9a9 71
username16248 9:92ddadd9e9a9 72
username16248 9:92ddadd9e9a9 73 void motor_r(int dir_r,float rot_target_r)
username16248 9:92ddadd9e9a9 74 {
username16248 9:92ddadd9e9a9 75 rot_prop_r=rot_target_r-rot_now_r;
username16248 9:92ddadd9e9a9 76 //rot_diff_r=rot_now_r-rot_old_r;
username16248 9:92ddadd9e9a9 77 rot_inte_r+=rot_prop_r;
username16248 9:92ddadd9e9a9 78 rot_PID_r=Kp_r*rot_prop_r+Kd_r*rot_diff_r;//+Ki_r*rot_inte;
username16248 9:92ddadd9e9a9 79 duty_PID_r=rot_PID_r*1.092f*0.0001f;
username16248 9:92ddadd9e9a9 80 duty_out_r=duty_out_r+duty_PID_r;
username16248 9:92ddadd9e9a9 81
username16248 9:92ddadd9e9a9 82 if(duty_out_r>DUTY_MAX) {
username16248 9:92ddadd9e9a9 83 duty_out_r=DUTY_MAX;
username16248 9:92ddadd9e9a9 84 } else if(duty_out_r<0) {
username16248 9:92ddadd9e9a9 85 duty_out_r=0;
username16248 9:92ddadd9e9a9 86 }
username16248 9:92ddadd9e9a9 87
username16248 9:92ddadd9e9a9 88 if(dir_r==1) {
username16248 9:92ddadd9e9a9 89 wheelr1=duty_out_r;
username16248 9:92ddadd9e9a9 90 wheelr2=0;
username16248 9:92ddadd9e9a9 91 } else if(dir_r==-1) {
username16248 9:92ddadd9e9a9 92 wheelr1=0;
username16248 9:92ddadd9e9a9 93 wheelr2=duty_out_r;
username16248 9:92ddadd9e9a9 94 } else {
username16248 9:92ddadd9e9a9 95 wheelr1=0;
username16248 9:92ddadd9e9a9 96 wheelr2=0;
username16248 9:92ddadd9e9a9 97 }
username16248 9:92ddadd9e9a9 98 }
username16248 9:92ddadd9e9a9 99
username16248 9:92ddadd9e9a9 100 void motor_l(int dir_l,float rot_target_l)
username16248 9:92ddadd9e9a9 101 {
username16248 9:92ddadd9e9a9 102 rot_prop_l=rot_target_l-rot_now_l;
username16248 9:92ddadd9e9a9 103 //rot_diff_l=rot_now_l-rot_old_l;
username16248 9:92ddadd9e9a9 104 rot_inte_l+=rot_prop_l;
username16248 9:92ddadd9e9a9 105 rot_PID_l=Kp_l*rot_prop_l+Kd_l*rot_diff_l;//+Ki_l*rot_inte;
username16248 9:92ddadd9e9a9 106 duty_PID_l=rot_PID_l*1.092f*0.0001f;
username16248 9:92ddadd9e9a9 107 duty_out_l=duty_out_l+duty_PID_l;
username16248 9:92ddadd9e9a9 108
username16248 9:92ddadd9e9a9 109 if(duty_out_l>DUTY_MAX) {
username16248 9:92ddadd9e9a9 110 duty_out_l=DUTY_MAX;
username16248 9:92ddadd9e9a9 111 } else if(duty_out_l<0) {
username16248 9:92ddadd9e9a9 112 duty_out_l=0;
username16248 9:92ddadd9e9a9 113 }
username16248 9:92ddadd9e9a9 114
username16248 9:92ddadd9e9a9 115 if(dir_l==1) {
username16248 9:92ddadd9e9a9 116 wheell1=duty_out_l;
username16248 9:92ddadd9e9a9 117 wheell2=0;
username16248 9:92ddadd9e9a9 118 } else if(dir_l==-1) {
username16248 9:92ddadd9e9a9 119 wheell1=0;
username16248 9:92ddadd9e9a9 120 wheell2=duty_out_l;
username16248 9:92ddadd9e9a9 121 } else {
username16248 9:92ddadd9e9a9 122 wheell1=0;
username16248 9:92ddadd9e9a9 123 wheell2=0;
username16248 9:92ddadd9e9a9 124 }
username16248 9:92ddadd9e9a9 125 }
username16248 9:92ddadd9e9a9 126 ///////////////////////////////////////////////////////////////
username16248 9:92ddadd9e9a9 127
username16248 9:92ddadd9e9a9 128 void motor_target(double rot_target_f,double rot_target_r,double rot_target_l)
username16248 9:92ddadd9e9a9 129 {
username16248 9:92ddadd9e9a9 130 ////////////////////前、回転方向決定/////////////////////
username16248 9:92ddadd9e9a9 131 if(rot_target_f>0) {
username16248 9:92ddadd9e9a9 132 dir_f=1;
username16248 9:92ddadd9e9a9 133 } else if(rot_target_f<0) {
username16248 9:92ddadd9e9a9 134 dir_f=-1;
username16248 9:92ddadd9e9a9 135 rot_target_f=-rot_target_f;
username16248 9:92ddadd9e9a9 136 } else {
username16248 9:92ddadd9e9a9 137 dir_f=0;
username16248 9:92ddadd9e9a9 138 }
username16248 9:92ddadd9e9a9 139 motor_f(dir_f,rot_target_f);
username16248 9:92ddadd9e9a9 140 ////////////////////右、回転方向決定/////////////////////
username16248 9:92ddadd9e9a9 141 if(rot_target_r>0) {
username16248 9:92ddadd9e9a9 142 dir_r=1;
username16248 9:92ddadd9e9a9 143 } else if(rot_target_r<0) {
username16248 9:92ddadd9e9a9 144 dir_r=-1;
username16248 9:92ddadd9e9a9 145 rot_target_r=-rot_target_r;
username16248 9:92ddadd9e9a9 146 } else {
username16248 9:92ddadd9e9a9 147 dir_r=0;
username16248 9:92ddadd9e9a9 148 }
username16248 9:92ddadd9e9a9 149 motor_r(dir_r,rot_target_r);
username16248 9:92ddadd9e9a9 150 ////////////////////左、回転方向決定/////////////////////
username16248 9:92ddadd9e9a9 151 if(rot_target_l>0) {
username16248 9:92ddadd9e9a9 152 dir_l=1;
username16248 9:92ddadd9e9a9 153 } else if(rot_target_l<0) {
username16248 9:92ddadd9e9a9 154 dir_l=-1;
username16248 9:92ddadd9e9a9 155 rot_target_l=-rot_target_l;
username16248 9:92ddadd9e9a9 156 } else {
username16248 9:92ddadd9e9a9 157 dir_l=0;
username16248 9:92ddadd9e9a9 158 }
username16248 9:92ddadd9e9a9 159 motor_l(dir_l,rot_target_l);
username16248 9:92ddadd9e9a9 160 }
username16248 9:92ddadd9e9a9 161
username16248 9:92ddadd9e9a9 162
username16248 9:92ddadd9e9a9 163 //////////////////////////////////////////////////////////////////////
username16248 9:92ddadd9e9a9 164 //////////////////////////////////回転数計算/////////////////////////////////
username16248 9:92ddadd9e9a9 165 void rotation_cal()
username16248 9:92ddadd9e9a9 166 {
username16248 9:92ddadd9e9a9 167 ////////////////////////motor_f////////////////////////
username16248 9:92ddadd9e9a9 168 angle_f=count_f/resolution*2.0f*3.14f;
username16248 9:92ddadd9e9a9 169 omega_now_f=abs(angle_f-angle_old_f)/0.01f;
username16248 9:92ddadd9e9a9 170 rot_now_f=omega_now_f*10/(2.0f*3.14f);
username16248 9:92ddadd9e9a9 171 rot_diff_f=rot_now_f-rot_old_f;
username16248 9:92ddadd9e9a9 172 angle_old_f=angle_f;
username16248 9:92ddadd9e9a9 173 rot_old_f=rot_now_f;
username16248 9:92ddadd9e9a9 174 ////////////////////////motor_r////////////////////////
username16248 9:92ddadd9e9a9 175 angle_r=count_r/resolution*2.0f*3.14f;
username16248 9:92ddadd9e9a9 176 omega_now_r=abs(angle_r-angle_old_r)/0.01f;
username16248 9:92ddadd9e9a9 177 rot_now_r=omega_now_r*10/(2.0f*3.14f);
username16248 9:92ddadd9e9a9 178 rot_diff_r=rot_now_r-rot_old_r;
username16248 9:92ddadd9e9a9 179 angle_old_r=angle_r;
username16248 9:92ddadd9e9a9 180 rot_old_r=rot_now_r;
username16248 9:92ddadd9e9a9 181 ////////////////////////motor_l////////////////////////
username16248 9:92ddadd9e9a9 182 angle_l=count_l/resolution*2.0f*3.14f;
username16248 9:92ddadd9e9a9 183 omega_now_l=abs(angle_l-angle_old_l)/0.01f;
username16248 9:92ddadd9e9a9 184 rot_now_l=omega_now_l*10/(2.0f*3.14f);
username16248 9:92ddadd9e9a9 185 rot_diff_l=rot_now_l-rot_old_l;
username16248 9:92ddadd9e9a9 186 angle_old_l=angle_l;
username16248 9:92ddadd9e9a9 187 rot_old_l=rot_now_l;
username16248 9:92ddadd9e9a9 188 //t++;
username16248 9:92ddadd9e9a9 189 }
username16248 9:92ddadd9e9a9 190
username16248 9:92ddadd9e9a9 191 void motor_setting()
username16248 9:92ddadd9e9a9 192 {
username16248 9:92ddadd9e9a9 193 //////////////////////////////モーターの設定をここに書いてください////////////////////////
username16248 9:92ddadd9e9a9 194 wheelf1.period_us(50);
username16248 9:92ddadd9e9a9 195 wheelf2.period_us(50);
username16248 9:92ddadd9e9a9 196 wheelr1.period_us(50);
username16248 9:92ddadd9e9a9 197 wheelr2.period_us(50);
username16248 9:92ddadd9e9a9 198 wheell1.period_us(50);
username16248 9:92ddadd9e9a9 199 wheell2.period_us(50);
username16248 9:92ddadd9e9a9 200 rot_cal.attach(&rotation_cal,0.001);
username16248 9:92ddadd9e9a9 201 }
username16248 9:92ddadd9e9a9 202