新部内対抗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班

function.h@12:2450012ce888, 2016-04-04 (annotated)

Committer:
naoakiarimoto
Date:
Mon Apr 04 09:25:57 2016 +0000
Revision:
12:2450012ce888
Child:
14:3403ce49a37a
add pin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
naoakiarimoto 12:2450012ce888 1 // pid,valve,interrupt,setupを定義
naoakiarimoto 12:2450012ce888 2
naoakiarimoto 12:2450012ce888 3 //PID
naoakiarimoto 12:2450012ce888 4 void pid(float t,int count,double rate,bool c){
naoakiarimoto 12:2450012ce888 5 target=t;
naoakiarimoto 12:2450012ce888 6 dist_n=2*PI*radius*rate*count/4096.0f;
naoakiarimoto 12:2450012ce888 7
naoakiarimoto 12:2450012ce888 8 dt=count_time.read();
naoakiarimoto 12:2450012ce888 9 count_time.reset();
naoakiarimoto 12:2450012ce888 10 count_time.start();
naoakiarimoto 12:2450012ce888 11
naoakiarimoto 12:2450012ce888 12 devi=t-dist_n;
naoakiarimoto 12:2450012ce888 13 diff=(dist_n-dist_p)/dt;
naoakiarimoto 12:2450012ce888 14 inte+=(dist_n-dist_p)*dt;
naoakiarimoto 12:2450012ce888 15
naoakiarimoto 12:2450012ce888 16 dist_p=dist_n;
naoakiarimoto 12:2450012ce888 17
naoakiarimoto 12:2450012ce888 18
naoakiarimoto 12:2450012ce888 19 pwm=Kp_a*devi+Kd_a*diff+Ki_a*inte;
naoakiarimoto 12:2450012ce888 20 pwm/=ratio;
naoakiarimoto 12:2450012ce888 21
naoakiarimoto 12:2450012ce888 22 if(pwm>MAX){
naoakiarimoto 12:2450012ce888 23 pwm=MAX;
naoakiarimoto 12:2450012ce888 24 }else if(pwm<-MAX){
naoakiarimoto 12:2450012ce888 25 pwm=-MAX;
naoakiarimoto 12:2450012ce888 26 }
naoakiarimoto 12:2450012ce888 27
naoakiarimoto 12:2450012ce888 28 if(!c){ //幹
naoakiarimoto 12:2450012ce888 29 if(pwm>=0){
naoakiarimoto 12:2450012ce888 30 bole_f=pwm;
naoakiarimoto 12:2450012ce888 31 bole_b=0;
naoakiarimoto 12:2450012ce888 32 }else{
naoakiarimoto 12:2450012ce888 33 bole_f=0;
naoakiarimoto 12:2450012ce888 34 bole_b=-pwm;
naoakiarimoto 12:2450012ce888 35 }
naoakiarimoto 12:2450012ce888 36 }else{ //桜
naoakiarimoto 12:2450012ce888 37 if(pwm>=0){
naoakiarimoto 12:2450012ce888 38 bloss_f=pwm;
naoakiarimoto 12:2450012ce888 39 bloss_b=0;
naoakiarimoto 12:2450012ce888 40 }else{
naoakiarimoto 12:2450012ce888 41 bloss_f=0;
naoakiarimoto 12:2450012ce888 42 bloss_b=-pwm;
naoakiarimoto 12:2450012ce888 43 }
naoakiarimoto 12:2450012ce888 44 }
naoakiarimoto 12:2450012ce888 45 }
naoakiarimoto 12:2450012ce888 46
naoakiarimoto 12:2450012ce888 47
naoakiarimoto 12:2450012ce888 48 //valve
naoakiarimoto 12:2450012ce888 49 void valve(bool c){
naoakiarimoto 12:2450012ce888 50 valve1=c;
naoakiarimoto 12:2450012ce888 51 // if(c){
naoakiarimoto 12:2450012ce888 52 wait(1);
naoakiarimoto 12:2450012ce888 53 // }
naoakiarimoto 12:2450012ce888 54 }
naoakiarimoto 12:2450012ce888 55
naoakiarimoto 12:2450012ce888 56
naoakiarimoto 12:2450012ce888 57 //interrupt-bole
naoakiarimoto 12:2450012ce888 58 void risea(){
naoakiarimoto 12:2450012ce888 59 pa=1;
naoakiarimoto 12:2450012ce888 60 if(pb==0) {
naoakiarimoto 12:2450012ce888 61 count++;
naoakiarimoto 12:2450012ce888 62 }else {
naoakiarimoto 12:2450012ce888 63 count--;
naoakiarimoto 12:2450012ce888 64 }
naoakiarimoto 12:2450012ce888 65 }
naoakiarimoto 12:2450012ce888 66 void falla(){
naoakiarimoto 12:2450012ce888 67 pa=0;
naoakiarimoto 12:2450012ce888 68 if(pb==0) {
naoakiarimoto 12:2450012ce888 69 count--;
naoakiarimoto 12:2450012ce888 70 }else {
naoakiarimoto 12:2450012ce888 71 count++;
naoakiarimoto 12:2450012ce888 72 }
naoakiarimoto 12:2450012ce888 73 }
naoakiarimoto 12:2450012ce888 74 void riseb(){
naoakiarimoto 12:2450012ce888 75 pb=1;
naoakiarimoto 12:2450012ce888 76 if(pa==0) {
naoakiarimoto 12:2450012ce888 77 count--;
naoakiarimoto 12:2450012ce888 78 }else {
naoakiarimoto 12:2450012ce888 79 count++;
naoakiarimoto 12:2450012ce888 80 }
naoakiarimoto 12:2450012ce888 81 }
naoakiarimoto 12:2450012ce888 82 void fallb(){
naoakiarimoto 12:2450012ce888 83 pb=0;
naoakiarimoto 12:2450012ce888 84 if(pa==0) {
naoakiarimoto 12:2450012ce888 85 count++;
naoakiarimoto 12:2450012ce888 86 }else {
naoakiarimoto 12:2450012ce888 87 count--;
naoakiarimoto 12:2450012ce888 88 }
naoakiarimoto 12:2450012ce888 89 }
naoakiarimoto 12:2450012ce888 90
naoakiarimoto 12:2450012ce888 91
naoakiarimoto 12:2450012ce888 92 //interrupt-blos
naoakiarimoto 12:2450012ce888 93 void risea2()
naoakiarimoto 12:2450012ce888 94 {
naoakiarimoto 12:2450012ce888 95 pa2=1;
naoakiarimoto 12:2450012ce888 96 if(pb2==0) {
naoakiarimoto 12:2450012ce888 97 count2++;
naoakiarimoto 12:2450012ce888 98 }
naoakiarimoto 12:2450012ce888 99
naoakiarimoto 12:2450012ce888 100 else {
naoakiarimoto 12:2450012ce888 101 count2--;
naoakiarimoto 12:2450012ce888 102 }
naoakiarimoto 12:2450012ce888 103 }
naoakiarimoto 12:2450012ce888 104 void falla2()
naoakiarimoto 12:2450012ce888 105 {
naoakiarimoto 12:2450012ce888 106 pa2=0;
naoakiarimoto 12:2450012ce888 107 if(pb2==0) {
naoakiarimoto 12:2450012ce888 108 count2--;
naoakiarimoto 12:2450012ce888 109 }
naoakiarimoto 12:2450012ce888 110
naoakiarimoto 12:2450012ce888 111 else {
naoakiarimoto 12:2450012ce888 112 count2++;
naoakiarimoto 12:2450012ce888 113 }
naoakiarimoto 12:2450012ce888 114 }
naoakiarimoto 12:2450012ce888 115 void riseb2()
naoakiarimoto 12:2450012ce888 116 {
naoakiarimoto 12:2450012ce888 117 pb2=1;
naoakiarimoto 12:2450012ce888 118 if(pa2==0) {
naoakiarimoto 12:2450012ce888 119 count2--;
naoakiarimoto 12:2450012ce888 120 }
naoakiarimoto 12:2450012ce888 121
naoakiarimoto 12:2450012ce888 122 else {
naoakiarimoto 12:2450012ce888 123 count2++;
naoakiarimoto 12:2450012ce888 124 }
naoakiarimoto 12:2450012ce888 125 }
naoakiarimoto 12:2450012ce888 126 void fallb2()
naoakiarimoto 12:2450012ce888 127 {
naoakiarimoto 12:2450012ce888 128 pb2=0;
naoakiarimoto 12:2450012ce888 129 if(pa2==0) {
naoakiarimoto 12:2450012ce888 130 count2++;
naoakiarimoto 12:2450012ce888 131 }
naoakiarimoto 12:2450012ce888 132
naoakiarimoto 12:2450012ce888 133 else {
naoakiarimoto 12:2450012ce888 134 count2--;
naoakiarimoto 12:2450012ce888 135 }
naoakiarimoto 12:2450012ce888 136 }
naoakiarimoto 12:2450012ce888 137
naoakiarimoto 12:2450012ce888 138 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
naoakiarimoto 12:2450012ce888 139 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
naoakiarimoto 12:2450012ce888 140
naoakiarimoto 12:2450012ce888 141
naoakiarimoto 12:2450012ce888 142
naoakiarimoto 12:2450012ce888 143
naoakiarimoto 12:2450012ce888 144
naoakiarimoto 12:2450012ce888 145 void setup(){
naoakiarimoto 12:2450012ce888 146
naoakiarimoto 12:2450012ce888 147 //interrupt
naoakiarimoto 12:2450012ce888 148 a.rise(&risea); //A interrupt
naoakiarimoto 12:2450012ce888 149 a.fall(&falla); //A interrupt
naoakiarimoto 12:2450012ce888 150 b.rise(&riseb); //B interrupt
naoakiarimoto 12:2450012ce888 151 b.fall(&fallb); //B interrupt
naoakiarimoto 12:2450012ce888 152
naoakiarimoto 12:2450012ce888 153 a2.rise(&risea2); //A2 interrupt
naoakiarimoto 12:2450012ce888 154 a2.fall(&falla2); //A2 interrupt
naoakiarimoto 12:2450012ce888 155 b2.rise(&riseb2); //B2 interrupt
naoakiarimoto 12:2450012ce888 156 b2.fall(&fallb2); //B2 interrupt
naoakiarimoto 12:2450012ce888 157
naoakiarimoto 12:2450012ce888 158 count=0;
naoakiarimoto 12:2450012ce888 159 count2=0;
naoakiarimoto 12:2450012ce888 160
naoakiarimoto 12:2450012ce888 161
naoakiarimoto 12:2450012ce888 162 //set-motor.period
naoakiarimoto 12:2450012ce888 163 bole_f.period_us(50);
naoakiarimoto 12:2450012ce888 164 bole_b.period_us(50);
naoakiarimoto 12:2450012ce888 165 bloss_f.period_us(50);
naoakiarimoto 12:2450012ce888 166 bloss_b.period_us(50);
naoakiarimoto 12:2450012ce888 167 catch1_f.period_us(50);
naoakiarimoto 12:2450012ce888 168 catch1_b.period_us(50);
naoakiarimoto 12:2450012ce888 169 catch2_f.period_us(50);
naoakiarimoto 12:2450012ce888 170 catch2_b.period_us(50);
naoakiarimoto 12:2450012ce888 171
naoakiarimoto 12:2450012ce888 172 //touchsensor-pullup
naoakiarimoto 12:2450012ce888 173 sw1.mode(PullUp);
naoakiarimoto 12:2450012ce888 174 sw2.mode(PullUp);
naoakiarimoto 12:2450012ce888 175
naoakiarimoto 12:2450012ce888 176 }
naoakiarimoto 12:2450012ce888 177