Revision 2:955cdadf5ecc, committed 2013-09-05

Comitter:

yusuke_robocup

Date:

Thu Sep 05 09:57:22 2013 +0000

Parent:

1:f465d89a26b0

Commit message:

aa

Changed in this revision

--- a/main.cpp	Thu Aug 01 09:05:23 2013 +0000
+++ b/main.cpp	Thu Sep 05 09:57:22 2013 +0000
@@ -8,9 +8,9 @@
 //#define ROTATE_PER_REVOLUTIONS  360//enko-da no bunkainou
 
 //QEI wheel(p23/*A*/, p24/*B*/, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
-HMC6352 compass(p28/*sda*/, p27/*scl*/);
-Servo myservo1(p21);
-Servo myservo2(p22);
+//HMC6352 compass(p28/*sda*/, p27/*scl*/);
+Servo servoR(p21);
+Servo servoL(p22);
 DigitalOut myled(LED1);
 Ticker interrupt;
 
@@ -61,37 +61,78 @@
     
     return m;
 }*/
+ 
+#define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
 
+#define STRAIGHT 0.6;
+#define SPIN 0.4;
+
+//#define STRAIGHT 0.0;
+//#define SPIN 1.0;
+
+
+void move(int vl,int vs){
+    double fut_R,fut_L,true_vs;
+    
+    true_vs = abs(vs)/SPIN;
+    
+    if(true_vs > 40){
+        vl = 0;
+        vs = 100*(vs/abs(vs));
+    }
+    
+    fut_R = Convert_dekaruto((vl + vs));
+    fut_L = Convert_dekaruto(-(vl - vs)*1.4);
+ 
+    servoR = fut_R;
+    servoL = fut_L;
+       
+    //printf("R:%lf   L:%lf\n",fut_R,fut_L);
+    //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
+}
+
+void PidUpdate()
+{   
+    inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
+    //printf("%lf\n",inputPID);      
+}
+
+double vsOut(){
+    double vs;
+    vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
+    vs = vs * 8;
+    
+    if(vs/abs(vs) < 0){
+        //vs *= 1.3;   
+    }
+    
+    if(abs(vs) > 90)vs = 90*(vs/abs(vs));
+    if(abs(vs) < 25) vs = 25*(vs/abs(vs));
+    
+    return vs;
+}
 
 
 int main() {
-    
-    
-    
-    
-    //printf("test\n");
+
+    wait(3);
+
+    int vl;
+    double vs;
     
     timer2.start();
-    interrupt.attach(&tic_sensor, 0.1/*sec*/);
+    //interrupt.attach(&tic_sensor, 0.1/*sec*/);
     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    pidUpdata.attach(&PidUpdate, PID_CYCLE);
     //printf("test%d\n",com_val);
     
     while(1) {
-    wait(0.1);
-        
+        vl = -90;
+        //vl = 0;
+    
+        vl = vl      * STRAIGHT;
+        vs = vsOut() * SPIN;
         
-      //  printf("pid:%.5d\n", ultrasonicVal[0]);
-        printf("Heading is: %f\n", compass.sample() / 10.0);
-
-        if((ultrasonicVal[0] < 1000)&&(ultrasonicVal[1] < 1000)){
-            myservo1 = 0.5;
-            myservo2 = 0.5;
-        }else if((ultrasonicVal[0] < ultrasonicVal[1])&&(ultrasonicVal[0] < 1000)){
-            myservo1 = 0.7 + proportional;
-            myservo2 = 0.7;
-        }else if((ultrasonicVal[0] > ultrasonicVal[1])&&(ultrasonicVal[1] < 1000)){
-            myservo1 = 0.3;
-            myservo2 = 0.3 + proportional;
-        }
+        move(vl,(int)vs); 
     }
 }

--- a/main.h	Thu Aug 01 09:05:23 2013 +0000
+++ b/main.h	Thu Sep 05 09:57:22 2013 +0000
@@ -4,4 +4,96 @@
 
 extern double ultrasonicValue[4];
 extern uint16_t ultrasonicVal[4];
-extern void Ultrasonic(void);
\ No newline at end of file
+extern void Ultrasonic(void);
+
+/* robocup */
+
+#define RATE    0.052
+#define Long    1.0
+#define ENTER   0
+#define EXIT    1
+#define X       0
+#define Y       1
+#define MOT_NUM 4
+#define MOTDRIVER_WAIT  300 //ms
+#define BAUD_RATE       115200
+#define BAUD_RATE2      19200
+#define BUT_WAIT        0.3 //s
+#define ON      1
+#define OFF     0
+
+#define PING_ERROR  0xFFFF
+#define PI          3.14159265
+
+#define MOT1    1.0
+#define MOT2    1.0
+#define MOT3    1.0
+#define MOT4    1.0
+
+#define PID_BIAS    0.0
+#define REFERENCE   180.0
+#define MINIMUM     0.0
+#define MAXIMUM     360.0
+
+#define PID_CYCLE   0.06    //s
+
+#define P_GAIN  0.75    //0.78   
+#define I_GAIN  0.0     //0.0
+#define D_GAIN  0.006   //0.009
+    
+#define OUT_LIMIT   30.0
+#define MAX_POW     100
+#define MIN_POW     -100
+
+DigitalOut led1(LED1); 
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+BusOut mbedleds(LED4,LED3,LED2,LED1); 
+HMC6352 compass(p28, p27);
+//Serial driver(p28, p27);    // tx, rx 
+Serial device2(p13, p14);   // tx, rx
+Serial pc(USBTX, USBRX);    // tx, rx 
+//DigitalIn StartButton(p21);
+//DigitalIn CalibEnterButton(p22);
+DigitalIn CalibExitButton(p23);
+DigitalIn EEPROMButton(p24);
+//PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);      //battery is low power version 30.0 0.58 1.0 0.015    battery is high power version   30.0 0.42, 1.0, 0.013   power is low but perfect 20.0 0.45, 0.0, 0.010
+Ticker pidUpdata;
+Ticker irDistanceUpdata;
+Timer timer1;
+Timer Survtimer;
+LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
+
+enum{
+    NORMAL,
+    LEFT_OUT,
+    RIGHT_OUT,
+    FRONT_OUT,
+    BACK_OUT,
+};
+
+enum{
+    HOME_WAIT,
+    DIFFENCE,
+    WARNING,
+    HOLD,
+};
+
+PinName adc_num[6] = {
+    p15,
+    p16,
+    p17,
+    p18,
+    p19,
+    p20,
+};
+double standTu = 0;
+int speed[MOT_NUM] = {0};
+uint8_t hold_flag = 0;
+uint8_t state = HOME_WAIT;
+uint8_t lineState = NORMAL;
+
+double inputPID = 180.0;
+static double standard;
+double compassPID = 0.0;
\ No newline at end of file