Revision 14:3403ce49a37a, committed 2016-04-05

Comitter:

baba2357

Date:

Tue Apr 05 17:25:09 2016 +0000

Parent:

13:7335d90b551a

Commit message:

??????;

Changed in this revision

--- a/arm_updown.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/arm_updown.h	Tue Apr 05 17:25:09 2016 +0000
@@ -3,7 +3,10 @@
 // (pc5.pc6.pa8.pa9)以外のpinを使う
 
 #include "mbed.h"
-
+//赤外線・超音波
+AnalogIn IR_r(PA_1);
+AnalogIn IR_l(PB_1);
+DigitalInOut uw(PC_4);
 #define PI 3.14159265f  //円周率
 
 //bole target
@@ -59,14 +62,12 @@
 #include "decision.h"       //幹・桜を感知する関数の定義（タイマー割り込み使用）
 #include "function.h"       //pid,valve,interrupt,setup関数を定義
 #include "step.h"           //各作業段階の動作を定義
-
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 
-
+/*
 int main(){
-    
     setup();
     
     //set-Ticker
@@ -87,7 +88,7 @@
         }else if(n>2){
             expand_sf();
         }
-/*
+
 //自己位置による条件
         if(){//雑木林
             set_bole();
@@ -100,10 +101,11 @@
         if(){//桜
             blossom();
         }
-*/
+
 
         pc.printf("|%d|\t%d\t%f\t%f\t%f\n\r",target,count,dist_n,devi,pwm);
         wait(0.1);
     }
 
 }
+*/
\ No newline at end of file

--- a/decision.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/decision.h	Tue Apr 05 17:25:09 2016 +0000
@@ -2,7 +2,6 @@
 // bole:測距     bloss:タッチ
 
 
-
 typedef struct{
     int r;
     int l;
@@ -21,11 +20,11 @@
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 
-
+/*
 void state_check() {      //タッチセンサ確認
 
-    sw1_n=sw1;
-    sw2_n=sw2;
+         sw1_n=sw1;
+         sw2_n=sw2;
     
     if(sw1_n*sw1_p*sw2_n*sw2_p) {
         sw_state=1;
@@ -43,7 +42,7 @@
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 
-
+*/
 void bole_check() {       //測距センサ確認
     double res;
 

--- a/encoder.cpp	Mon Apr 04 09:26:26 2016 +0000
+++ b/encoder.cpp	Tue Apr 05 17:25:09 2016 +0000
@@ -1,49 +1,49 @@
 #include "mbed.h"
 #include <pin_file.h>
 #include <encoder.h>
-InterruptIn b(EC1_b);//前部
-InterruptIn a(EC1_a);
+InterruptIn bb(EC1_b);//前部
+InterruptIn aa(EC1_a);
 InterruptIn d(EC2_b);//右部
 InterruptIn c(EC2_a);
 InterruptIn e(EC3_a);//左部
 InterruptIn f(EC3_b);
 
 int count_f=0,count_r=0,count_l=0;
-int pa,pb,pC,pd,pe,pf;
+int pA,pB,pC,pd,pe,pf;
 //int s=0;
 
 //interrupt
-void risea()
+void riseaa()
 {
-    pa=1;
-    if(pb==0) {
+    pA=1;
+    if(pB==0) {
         count_f++;
     } else {
         count_f--;
     }
 }
-void falla()
+void fallaa()
 {
-    pa=0;
-    if(pb==0) {
+    pA=0;
+    if(pB==0) {
         count_f--;
     } else {
         count_f++;
     }
 }
-void riseb()
+void risebb()
 {
-    pb=1;
-    if(pa==0) {
+    pB=1;
+    if(pA==0) {
         count_f--;
     } else {
         count_f++;
     }
 }
-void fallb()
+void fallbb()
 {
-    pb=0;
-    if(pa==0) {
+    pB=0;
+    if(pA==0) {
         count_f++;
     } else {
         count_f--;
@@ -128,10 +128,10 @@
 
 void encoder_setting(){
     //interrupt
-    a.rise(&risea);
-    a.fall(&falla);
-    b.rise(&riseb);
-    b.fall(&fallb);
+    aa.rise(&risea);
+    aa.fall(&falla);
+    bb.rise(&riseb);
+    bb.fall(&fallb);
     c.rise(&risec);
     c.fall(&fallc);
     d.rise(&rised);
@@ -141,5 +141,4 @@
     f.rise(&risef);
     f.fall(&fallf);
     //s++;
-    }
-
+    }
\ No newline at end of file

--- a/encoder.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/encoder.h	Tue Apr 05 17:25:09 2016 +0000
@@ -4,12 +4,22 @@
 
 
 extern int count_f,count_r,count_l;
-extern int pa,pb,pC,pd,pe,pf;
+extern int pA,pB,pC,pd,pe,pf;
 extern int s;
-
+void risea();
+void riseb();
+void risec();
+void falla();
+void fallb();
+void fallc();
+void rised();
+void risee();
+void risef();
+void falld();
+void falle();
+void fallf();
 void encoder_setting();
 
 #endif
 
 
-

--- a/function.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/function.h	Tue Apr 05 17:25:09 2016 +0000
@@ -1,5 +1,10 @@
 // pid,valve,interrupt,setupを定義
-
+//上下機構&アーム
+InterruptIn a(PC_8);
+InterruptIn b(PC_6);
+PwmOut bole_f(PB_4);
+PwmOut bole_b(PB_5);
+DigitalOut valve1(PB_7);
 //PID
 void pid(float t,int count,double rate,bool c){
     target=t;
@@ -34,7 +39,7 @@
             bole_b=-pwm;
         }
     }else{      //桜
-        if(pwm>=0){
+/*        if(pwm>=0){
             bloss_f=pwm;
             bloss_b=0;
         }else{
@@ -42,9 +47,10 @@
             bloss_b=-pwm;
         }    
     }
+*/
 }
 
-
+}
 //valve
 void valve(bool c){
     valve1=c;
@@ -90,6 +96,7 @@
 
 
 //interrupt-blos
+/*
 void risea2()
 {
     pa2=1;
@@ -134,7 +141,7 @@
         count2--;
     }
 }
-
+*/
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -150,19 +157,19 @@
     b.rise(&riseb);                       //B interrupt
     b.fall(&fallb);                       //B interrupt
 
-    a2.rise(&risea2);                     //A2 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;
-    
+/*    count2=0;
+*/    
     
     //set-motor.period
     bole_f.period_us(50);
     bole_b.period_us(50);
-    bloss_f.period_us(50);
+/*    bloss_f.period_us(50);
     bloss_b.period_us(50);
     catch1_f.period_us(50);
     catch1_b.period_us(50);
@@ -172,6 +179,6 @@
     //touchsensor-pullup
     sw1.mode(PullUp);
     sw2.mode(PullUp);
-    
+*/    
 }
 

--- a/main.cpp	Mon Apr 04 09:26:26 2016 +0000
+++ b/main.cpp	Tue Apr 05 17:25:09 2016 +0000
@@ -3,35 +3,55 @@
 #include "spi_nucleo.h"
 #include "transfer.h"
 #include <string.h>
-
+#include "arm_updown.h"
+#include "encoder.h"
+#include "speed_control.h"
+#include "math.h"
 int main(){
+    setup();
+    //set-Ticker
+    bole_checker.attach(bole_check,0.2);    
     spiInit();
     push.mode(PullUp);
     while(pushed_number==0){    //手動
         transfer();
         spi_mbed();     //コントローラーからの信号を読み取る
-        Cilinder=cilinder;
+        valve1=cilinder;
         if(RR==1){
-                    //右旋回の関数
+                motor_f(1,0.5);   //右旋回の関数
+                motor_r(1,0.5);
+                motor_l(1,0.5);
                 }
         else if(LR==1){
-                    //左旋回の関数
+                motor_f(-1,0.5);
+                motor_r(-1,0.5);
+                motor_l(-1,0.5);    //左旋回の関数
                 }
         else if(onoff==1){
-            if(updown==1) up=0.1;//上下機構上昇
-            else down=0.1;//上下機構下降
+            if(updown==1) bole_f=0.1;//上下機構上昇
+            else bole_b=0.1;//上下機構下降
                 }
         else{
+               motor_target(((-1)*speed_X)/30,((2/sqrt(3.0)*speed_X)-(speed_Y*2))/30,((2/sqrt(3.0)*speed_X)+(speed_Y*2))/30);
                     //読み取った値speed_X,speed_Yを速度制御にぶち込む
                 }
-        }
+            }
     while(pushed_number==1){    //自動
         transfer();
         getGyro();
             //自己位置
-            //エアシリンダー
+//測距関係により検知
+//エアシリンダー
+        if(UW*IR){   //幹検知
+            n++;
+            bole_checker.detach();
         }
-        
+        if(n==1){
+            expand_gf();
+        }else if(n==2){
+            expand_ff();
+        }else if(n>2){
+            expand_sf();
+        }
     }
-    
-
+}
\ No newline at end of file

--- a/pinMode.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/pinMode.h	Tue Apr 05 17:25:09 2016 +0000
@@ -1,4 +1,4 @@
-
+/*
 // pin番号・変数を定義
 // pid関数のプロトタイプ宣言
 
@@ -30,7 +30,7 @@
 
 
 Serial pc(USBTX, USBRX);
-
+*/
 
 //variable
 long int count=0,count2=0,pa,pb,pa2,pb2;

--- a/pin_file.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/pin_file.h	Tue Apr 05 17:25:09 2016 +0000
@@ -8,8 +8,8 @@
 #define EC2_b PC_13
 #define EC3_a PC_10
 #define EC3_b PC_12
-//InterruptIn a(PC_2);
-//InterruptIn b(PC_3);
+//InterruptIn aa(PC_2);
+//InterruptIn bb(PC_3);
 //InterruptIn c(PA_15);
 //InterruptIn d(PC_13);
 //InterruptIn e(PC_10);
@@ -25,26 +25,27 @@
 //PwmOut i(PB_6);
 //PwmOut j(PA_9);
 //PwmOut k(PA_8);
-//PwmOut l(PB_10);
+//PwmOut l(PB_10);b
 //自己位置
-InterruptIn m(PC_7);
-InterruptIn n(PC_11);
+/*InterruptIn m(PC_7);
+InterruptIn nn(PC_11);
 InterruptIn o(PC_1);
 InterruptIn p(PC_0);
 //上下機構&アーム
-InterruptIn q(PC_8);
-InterruptIn r(PC_6);
-PwmOut up(PB_4);
-PwmOut down(PB_5);
-DigitalOut Cilinder(PB_7);
+InterruptIn a(PC_8);
+InterruptIn b(PC_6);
+PwmOut bole_f(PB_4);
+PwmOut bole_b(PB_5);
+DigitalOut valve1(PB_7);
 //SPI mbed
 SPISlave from_mbed(PA_7, PA_6, PA_5, PA_4);
 //SPI NUCLEO
 SPISlave from_gyro(PB_15, PB_14, PB_13, PB_12);
 //赤外線・超音波
-AnalogIn v(PA_1);
-AnalogIn w(PB_1);
-DigitalInOut x(PC_4);
+AnalogIn IR_r(PA_1);
+AnalogIn IR_l(PB_1);
+DigitalInOut uw(PC_4);
 //自動切り替え
-DigitalIn y(PA_12);
+DigitalIn push(PA_12);
+*/
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/plot.h	Tue Apr 05 17:25:09 2016 +0000
@@ -0,0 +1,25 @@
+#ifndef PLOT_H
+#define PLOT_H
+
+
+extern double target_plot[][3]={
+    /*{-600,1200,-90},//青のとき
+    {-600,1500,-90},
+    {-600,1800,-90},
+    {900,1800,-90},
+    {900,1800,90},
+    {900,1800,0}
+    */
+    
+   
+    
+    {600,1200,90},//赤のとき location_state=1 一個目
+    {600,1500,90},//location_state=2 二個目
+    {600,1800,90},//location_state=3 三個目
+    {-900,1800,90},//雑木林
+    {-900,1800,-90},//雑木林に置く
+    //{-900,1800,0}//上面を向く
+    
+    };
+    
+#endif
\ No newline at end of file

--- a/speed_control.cpp	Mon Apr 04 09:26:26 2016 +0000
+++ b/speed_control.cpp	Tue Apr 05 17:25:09 2016 +0000
@@ -1,3 +1,4 @@
+
 #include "mbed.h"
 #include <pin_file.h>
 #include <speed_control.h>
@@ -11,7 +12,8 @@
 PwmOut wheell1(WHl1);//左部
 PwmOut wheell2(WHl2);
 Ticker rot_cal;
-/*float angle_f=0.0,angle_r=0.0,angle_l=0.0;
+
+float angle_f=0.0,angle_r=0.0,angle_l=0.0;
 float angle_old_f=0.0,angle_old_r=0.0,angle_old_l=0.0;
 float duty_out_f=0.0,duty_out_r=0.0,duty_out_l=0.0;
 float duty_PID_f=0.0,duty_PID_r=0.0,duty_PID_l=0.0;
@@ -27,8 +29,8 @@
 const float Kp_r=30.0000f,Kd_r=200.0000f,Ki_r=0.0001f;
 const float Kp_l=30.0000f,Kd_l=200.0000f,Ki_l=0.0001f;
 const float resolution=1024.0;
-int t=0;*/
-/*
+int t=0;
+
 void motor_move()
 {
     wheelf1=0.1;
@@ -38,7 +40,7 @@
     wheell1=0.1;
     wheell2=0;
 }
-*/
+
 
 
 ////////////////////////////モーターそれぞれのPID制御/////////////////////////

--- a/speed_control.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/speed_control.h	Tue Apr 05 17:25:09 2016 +0000
@@ -2,8 +2,8 @@
 #ifndef SPEED_CONTROL_H
 #define SPEED_CONTROL_H
 
-
-float angle_f=0.0,angle_r=0.0,angle_l=0.0;
+//Ticker rot_cal;
+/*float angle_f=0.0,angle_r=0.0,angle_l=0.0;
 float angle_old_f=0.0,angle_old_r=0.0,angle_old_l=0.0;
 float duty_out_f=0.0,duty_out_r=0.0,duty_out_l=0.0;
 float duty_PID_f=0.0,duty_PID_r=0.0,duty_PID_l=0.0;
@@ -20,30 +20,12 @@
 const float Kp_l=30.0000f,Kd_l=200.0000f,Ki_l=0.0001f;
 const float resolution=1024.0;
 int t=0;
-/*
-extern float angle_f,angle_r,angle_l;
-extern float angle_old_f,angle_old_r,angle_old_l;
-extern float duty_out_f,duty_out_r,duty_out_l;
-extern float duty_PID_f,duty_PID_r,duty_PID_l;
-extern float omega_now_f,omega_now_r,omega_now_l;
-extern float rot_now_f,rot_now_r,rot_now_l;
-extern float rot_old_f,rot_old_r,rot_old_l;
-extern float rot_PID_f,rot_PID_r,rot_PID_l;
-extern float rot_prop_f,rot_prop_r,rot_prop_l;
-extern float rot_diff_f,rot_diff_r,rot_diff_l;
-extern float rot_inte_f,rot_inte_r,rot_inte_l;
-extern int dir_f,dir_r,dir_l;
-
-extern int i;
-//int targetA,targetB,targetC;
-extern int ka,kb,kc;
-extern int df,dr,dl;
-
-extern const float Kp,Kd,Ki;
-extern const float resolution;
-
-extern int t;
 */
 void motor_setting();
+void rotation_cal();
+void motor_f(int dir_f,float rot_target_f);
+void motor_r(int dir_r,float rot_target_r);
+void motor_l(int dir_l,float rot_target_l);
+void motor_target(double rot_target_f,double rot_target_r,double rot_target_l);
 
 #endif
\ No newline at end of file

--- a/spi_nucleo.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/spi_nucleo.h	Tue Apr 05 17:25:09 2016 +0000
@@ -3,7 +3,10 @@
 #include <string.h>
 #ifndef SPI_NUCLEO_H
 #define SPI_NUCLEO_H
-
+//SPI mbed
+SPISlave from_mbed(PA_7, PA_6, PA_5, PA_4);
+//SPI NUCLEO
+SPISlave from_gyro(PB_15, PB_14, PB_13, PB_12);
 //SPI mbed
 bool RR=0,LR=0,Xpm=0,cilinder=0,onoff=0,updown=0,Ypm=0;
 int speed_X,speed_Y;

--- a/step.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/step.h	Tue Apr 05 17:25:09 2016 +0000
@@ -88,7 +88,7 @@
     }
 }
 
-
+/*
 void blossom(){      //桜を掴み、持ち上げる
     sw_check.attach(&state_check,0.2);
     while(1){
@@ -112,12 +112,13 @@
         if(HMIN<=devi&&devi<=HMAX){
             arm=5;
             }
-/*        if(){       //花見場
+        if(){       //花見場
             bloss1_f=0;
             bloss1_b=OPEN;
             bloss1_f=OPEN;
             bloss1_b=0;
             }
-*/
+*//*
         }
 }
+*/
\ No newline at end of file

--- a/transfer.h	Mon Apr 04 09:26:26 2016 +0000
+++ b/transfer.h	Tue Apr 05 17:25:09 2016 +0000
@@ -1,6 +1,6 @@
 #ifndef TRANSFER_H
 #define TRANSFER_H
-
+#include "pin_file.h"
 DigitalIn push(PA_12);
 bool pushed_number=0;
 void transfer();