新部内対抗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@14:3403ce49a37a, 2016-04-05 (annotated)

Committer:
baba2357
Date:
Tue Apr 05 17:25:09 2016 +0000
Revision:
14:3403ce49a37a
Parent:
11:f8fac9693cd5 
??????;

Who changed what in which revision?

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