Diff: main.cpp

Revision:

2:955cdadf5ecc

Parent:

1:f465d89a26b0

--- 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); 
     }
 }