Diff: main.cpp

Revision:

0:13ab960fc61f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,172 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+/***********  Serial interrupt  ***********/
+
+void Tx_interrupt()
+{
+    array(speed[0],speed[1],speed[2],speed[3]);
+    driver.printf("%s",StringFIN.c_str());
+    //pc.printf("%s",StringFIN.c_str());
+    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+}
+/*
+void Rx_interrupt()
+{
+    if(driver.readable()){
+        //pc.printf("%d\n",driver.getc());
+    }
+}*/
+
+
+/***********  Serial interrupt end **********/
+
+void PidUpdata()
+{
+    float deviation = 0;
+    
+    
+    if(standard < 180.0){
+        if((compass.sample() / 10.0) < standard){
+            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
+        }else if((compass.sample() / 10.0) < 180.0 + standard){
+            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
+        }else{
+            inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard);
+        }
+    }else if(standard > 180.0){
+        if((compass.sample() / 10.0) > standard){
+            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
+        }else if((compass.sample() / 10.0) > standard - 180.0){
+            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
+        }else{
+            inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard));
+        }
+    }else{
+        inputPID = compass.sample() / 10.0;
+    }
+    
+    if(inputPID < 0){
+        inputPID *= -1; //
+    }
+        
+    
+    //pid.setProcessValue(inputPID);
+    
+    deviation = (inputPID - 180.0);
+    
+    compassPID = ((deviation / 180.0) * 20.0 + ((deviation - lastData) / 2.0));
+    
+    if(compassPID > 100)compassPID = 100;
+    else if(compassPID < -100)compassPID = -100;
+    
+    //pc.printf("%f\n",compassPID);
+    
+    lastData = deviation;
+}
+
+
+
+void move(int vx, int vy, int vs)
+{
+    double motVal[MOT_NUM] = {0};
+    
+    motVal[0] = (double)((0.5 * vx)  + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
+    motVal[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
+    motVal[2] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
+    motVal[3] = (double)((0.5  * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
+    
+    for(uint8_t i = 0;i < MOT_NUM;i++){
+        if(motVal[i] > 100)motVal[i] = 100;
+        else if(motVal[i] < -100)motVal[i] = -100;
+        speed[i] = motVal[i];
+    }
+    /*
+    pc.printf("speed1 = %d\n",speed[0]);
+    pc.printf("speed2 = %d\n",speed[1]);
+    pc.printf("speed3 = %d\n",speed[2]);
+    pc.printf("speed4 = %d\n\n",speed[3]);      
+    */
+    ////pc.printf("%s",StringFIN.c_str());
+}
+
+void init()
+{
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    StartButton.mode(PullUp);
+    CalibEnterButton.mode(PullUp);
+    CalibExitButton.mode(PullUp);
+    driver.baud(BAUD_RATE);
+    wait_ms(MOTDRIVER_WAIT);
+    driver.printf("1F0002F0003F0004F000\r\n"); 
+    
+    led1 = ON;
+    
+    while(StartButton){
+        if(!CalibEnterButton){
+            led1 = OFF;
+            led2 = ON;
+            compass.setCalibrationMode(ENTER);
+            while(CalibExitButton);
+            compass.setCalibrationMode(EXIT);
+            led2 = OFF;
+            led3 = ON;
+         }
+    }
+    
+    standard = compass.sample() / 10.0;
+    led1 = OFF;
+    led3 = OFF;
+    
+    pid.setInputLimits(0.0, 360.0);
+    pid.setOutputLimits(-50.0, 50.0);
+    pid.setBias(0.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(180.0);
+    
+    pidUpdata.attach(&PidUpdata, 0.06);
+    //ultrasonic.attach(&Ultrasonic, 0.1);
+    IR.attach(&IR_Position,0.04);
+    driver.attach(&Tx_interrupt, Serial::TxIrq);
+    //driver.attach(&Rx_interrupt, Serial::RxIrq);
+    
+    timer2.start();
+}
+
+int main()
+{
+    int vx=0,vy=0,vs=0;
+    
+    init();
+           
+    while(1) {
+        vs = compassPID;
+        
+        if(IR_found){
+            if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
+                vx = (int)(20*ball_sankaku[direction][0]);
+                vy = (int)(20*ball_sankaku[direction][1]);
+            }else{
+                vx = (int)(10*ball_sankaku[direction][0]);
+                vy = (int)(10*ball_sankaku[direction][1]);
+            }
+            
+            if(Distance <= 10){
+                vx *= -1;
+                vy *= -1;
+            }
+        }else{
+            vx = 0;
+            vy = 0;
+        }
+        
+        
+        move(vx,vy,vs);
+    }
+}