新部内対抗A班
/
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班
Diff: function.h
- Revision:
- 12:2450012ce888
- Child:
- 14:3403ce49a37a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/function.h Mon Apr 04 09:25:57 2016 +0000
@@ -0,0 +1,177 @@
+// pid,valve,interrupt,setupを定義
+
+//PID
+void pid(float t,int count,double rate,bool c){
+ target=t;
+ dist_n=2*PI*radius*rate*count/4096.0f;
+
+ dt=count_time.read();
+ count_time.reset();
+ count_time.start();
+
+ devi=t-dist_n;
+ diff=(dist_n-dist_p)/dt;
+ inte+=(dist_n-dist_p)*dt;
+
+ dist_p=dist_n;
+
+
+ pwm=Kp_a*devi+Kd_a*diff+Ki_a*inte;
+ pwm/=ratio;
+
+ if(pwm>MAX){
+ pwm=MAX;
+ }else if(pwm<-MAX){
+ pwm=-MAX;
+ }
+
+ if(!c){ //幹
+ if(pwm>=0){
+ bole_f=pwm;
+ bole_b=0;
+ }else{
+ bole_f=0;
+ bole_b=-pwm;
+ }
+ }else{ //桜
+ if(pwm>=0){
+ bloss_f=pwm;
+ bloss_b=0;
+ }else{
+ bloss_f=0;
+ bloss_b=-pwm;
+ }
+ }
+}
+
+
+//valve
+void valve(bool c){
+ valve1=c;
+// if(c){
+ wait(1);
+// }
+}
+
+
+//interrupt-bole
+void risea(){
+ pa=1;
+ if(pb==0) {
+ count++;
+ }else {
+ count--;
+ }
+}
+void falla(){
+ pa=0;
+ if(pb==0) {
+ count--;
+ }else {
+ count++;
+ }
+}
+void riseb(){
+ pb=1;
+ if(pa==0) {
+ count--;
+ }else {
+ count++;
+ }
+}
+void fallb(){
+ pb=0;
+ if(pa==0) {
+ count++;
+ }else {
+ count--;
+ }
+}
+
+
+//interrupt-blos
+void risea2()
+{
+ pa2=1;
+ if(pb2==0) {
+ count2++;
+ }
+
+ else {
+ count2--;
+ }
+}
+void falla2()
+{
+ pa2=0;
+ if(pb2==0) {
+ count2--;
+ }
+
+ else {
+ count2++;
+ }
+}
+void riseb2()
+{
+ pb2=1;
+ if(pa2==0) {
+ count2--;
+ }
+
+ else {
+ count2++;
+ }
+}
+void fallb2()
+{
+ pb2=0;
+ if(pa2==0) {
+ count2++;
+ }
+
+ else {
+ count2--;
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+void setup(){
+
+ //interrupt
+ a.rise(&risea); //A interrupt
+ a.fall(&falla); //A interrupt
+ b.rise(&riseb); //B interrupt
+ b.fall(&fallb); //B interrupt
+
+ a2.rise(&risea2); //A2 interrupt
+ a2.fall(&falla2); //A2 interrupt
+ b2.rise(&riseb2); //B2 interrupt
+ b2.fall(&fallb2); //B2 interrupt
+
+ count=0;
+ count2=0;
+
+
+ //set-motor.period
+ bole_f.period_us(50);
+ bole_b.period_us(50);
+ bloss_f.period_us(50);
+ bloss_b.period_us(50);
+ catch1_f.period_us(50);
+ catch1_b.period_us(50);
+ catch2_f.period_us(50);
+ catch2_b.period_us(50);
+
+ //touchsensor-pullup
+ sw1.mode(PullUp);
+ sw2.mode(PullUp);
+
+}
+