Diff: main.cpp

Revision:

1:aea254b39529

Parent:

0:6bc5f85ca6fa

--- a/main.cpp	Tue May 21 13:48:10 2013 +0000
+++ b/main.cpp	Tue May 24 12:08:14 2016 +0000
@@ -1,69 +1,328 @@
 #include "mbed.h"
 #include "HMC5883L.h"
+#include <math.h>
+#include "rtos.h"
+#include <string>
+#define SDA_ARM      p28 //p9
+#define SCL_ARM      p27 //p10
+#define SDA_BODY      p9 //p28
+#define SCL_BODY      p10 //p27
+#define PI       3.14159265
+#define SERVO_PIN p21
+#define MaxDegreeDeviation 2 // how many degrees to ignore near north / target
+#define ARM_MOUNT_ANGLE -70
+#define BODY_MOUNT_ANGLE 0 //was -50
+#define CCW_MIN_SPEED 1540  //1550 
+#define CCW_MAX_SPEED 1800
 
-#include <math.h>
+#define CW_MIN_SPEED 1460  // 1450 
+#define CW_MAX_SPEED 1200
+
+//#define DEBUG_ARM_LOCK_TIMER
+//#define DEBUG_SERVO_PWM
+//#define DEBUG_GRAPH
+
+Serial pc(USBTX,USBRX);
+int TargetAngle = 0;
+float BODY_MOUNT_RAD_FIX =   BODY_MOUNT_ANGLE / 180.0 * PI;
+float Arm_RAD_FIX = (ARM_MOUNT_ANGLE/180.0*PI) - (TargetAngle/180.0 * PI);
+float declinationAngle = 0.069; // fix by position (radian) - netanya is 0.069 (3.6 deg)
+PwmOut Servo(SERVO_PIN);
+bool moveFlag = false;
+
+Serial BT(p13, p14);
+float degreeToNorthArm = 0,degreeToNorthBody=0;
+int bodyLastDegree = 0;
+float z,x,y;;
+HMC5883L hmc_arm(SDA_ARM, SCL_ARM);
+HMC5883L hmc_body(SDA_BODY, SCL_BODY);
+
+DigitalOut led2(LED2);
+
+Ticker BTtimer;
+Ticker Arm_timer;
+Timer ArmlockTimer;
+Timer globalTimer;
+
+//char globalChar = ' ';
+string globalString =" ";
+
+//prototypes:
+void BTsendDegrees(float degree);
+void BTsendInterval();
+void Arm_Ticker();
+void Read_Magnetic_arm();
+void Read_Magnetic_body();
+float GetDegreeToNorth(int sensor);
+
+
+
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
 
-#define SDA      p9
-#define SCL      p10
-#define PI       3.14159265
+void RotateClockWise(int moveSpeed)
+{
+    led2 = 1;
+    moveFlag = true;
+    int moveSpeedCalc = map(180-moveSpeed,MaxDegreeDeviation,180,CW_MAX_SPEED,CW_MIN_SPEED);
+    Servo.pulsewidth_us(moveSpeedCalc);
+   // Servo.pulsewidth(CW_MAX_SPEED);
+   // wait_ms(SERVO_WAIT_TRIGGERS);
+    //Servo.pulsewidth_us(1500);
+    
+    #ifdef DEBUG_SERVO_PWM
+    printf("CW  Servo Heading Speed : %d\tActualPwm: %.6f\n",moveSpeed,moveSpeedCalc);
+    #endif
+    
+    #ifdef DEBUG_ARM_LOCK_TIMER
+    ArmlockTimer.start();
+    #endif
+    
+    #ifdef DEBUG_GRAPH
+    printf("%d,%d,%d\n",globalTimer.read_ms(),moveSpeed,moveSpeedCalc);
+    #endif
+}
+
+
+void RotateCounterClockWise(int moveSpeed)
+{
+    led2 = 1;
+    moveFlag = true;
+    int moveSpeedCalc = map(abs(moveSpeed),MaxDegreeDeviation,180,CCW_MIN_SPEED,CCW_MAX_SPEED);
+    Servo.pulsewidth_us(moveSpeedCalc);
+   // Servo.pulsewidth(CCW_MAX_SPEED);
+   // wait_ms(SERVO_WAIT_TRIGGERS);
+    //Servo.pulsewidth_us(1500);
+    #ifdef DEBUG_SERVO_PWM
+    printf("CCW Servo Heading Speed : %d\tActualPwm: %.6f\n",moveSpeed,moveSpeedCalc);
+    #endif
+    #ifdef DEBUG_ARM_LOCK_TIMER
+    ArmlockTimer.start();
+    #endif
+    #ifdef DEBUG_GRAPH
+    printf("%d,%d,%d\n",globalTimer.read_ms(),moveSpeed,moveSpeedCalc);
+    #endif
+}
+
+void StopServo()
+{
+    led2 = 0;
+    //myServo.writeMicroseconds(1500);
+    Servo.pulsewidth(0.0015); // pulse width 1500ms - servo stop
+    #ifdef DEBUG_ARM_LOCK_TIMER
+    if(moveFlag)
+    {
+        ArmlockTimer.stop();     
+     //   printf("Arm locking time : %d ms\n",ArmlockTimer.read_ms());  // - Print the incoming command
+        ArmlockTimer.reset();
+        moveFlag = false;       
+    }
+    #endif  
+}
+
+
+void DataReceived_ISR(){
+     char globalChar = LPC_UART1->RBR;
+     globalString += globalChar; 
+}
+
+int getCommandValue(string cmd){
+    string cmdValue;   
+    std::size_t sharpPosition = cmd.find("#");  
+    cmdValue = cmd.substr(sharpPosition+1);
+ //   printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
+    return atoi(cmdValue.c_str());
+}
 
-int main()
-{      
-    float x, y, z, heading;
-    /*
-    float m_x, m_y, m_z;
-    */
-    printf("Inicializing...\r\n");
-    HMC5883L hmc5883l(SDA, SCL);
-    //HMC5883L *hmc5883l = new HMC5883L(SDA, SCL);
-    printf("OK...\r\n");
-    /*
-    for (int i = 0 ; i < 100 ; i++)
+void BTCommands (string cmd)
+{
+   
+   //printf("Input = %s\n",globalString);  // - Print the incoming command
+    
+   if (cmd.find("Target")!= string::npos)
+   {
+       TargetAngle = getCommandValue(cmd);
+       Arm_RAD_FIX = Arm_RAD_FIX = (ARM_MOUNT_ANGLE/180.0*PI) - (TargetAngle/180.0 * PI);
+     //  printf("New Target Angle set to: %d\n",TargetAngle);
+   }
+      
+}
+
+void thread1(void const *args) //arm thread
+{
+    while(true) {
+        degreeToNorthArm = GetDegreeToNorth(2); // 2 is arm sensor
+
+        if(degreeToNorthArm > MaxDegreeDeviation)
+            RotateClockWise(degreeToNorthArm);                   
+        else if(degreeToNorthArm < -MaxDegreeDeviation)
+            RotateCounterClockWise(degreeToNorthArm);
+                      
+        else  
+          StopServo();  
+                         
+        Thread::wait(10);
+    }
+}
+
+void thread2(void const *args) //body thread
+{
+    while(true) { 
+    degreeToNorthBody = GetDegreeToNorth(1);
+    if(abs(bodyLastDegree - degreeToNorthBody) > 2)
+    {     
+       // printf("degrees: %f\n ",degreeToNorthBody);
+        BTsendDegrees(degreeToNorthBody);
+        bodyLastDegree = degreeToNorthBody;
+        Thread::wait(800);     
+    }
+    else  
+        Thread::wait(800);
+    }
+}
+
+
+
+void thread3(void const *args)
+{
+    while(true) { 
+        if(globalString.length() > 0 )
+        {
+            BTCommands(globalString);
+            globalString = "";
+        }
+        
+        Thread::wait(100);
+    }
+}
+
+void BTsendDegrees(float degree)
+{
+      BT.printf("#%f~",degree);
+}
+
+void BTsendInterval()
+{
+    degreeToNorthBody = GetDegreeToNorth(1);
+    BTsendDegrees(degreeToNorthBody);
+   // Arm_timer.attach(Arm_Ticker, 0.0001); // activate arm sensor read frequency
+}
+
+void Read_Magnetic_arm()
+{
+    x = hmc_arm.getMx();
+    y = hmc_arm.getMy();
+    z = hmc_arm.getMz();
+}
+
+void Read_Magnetic_body()
+{
+    x = hmc_body.getMx();
+    y = hmc_body.getMy();
+    z = hmc_body.getMz();
+}
+
+float GetDegreeToNorth(int sensor)
+{
+    float heading;
+     
+       
+    switch(sensor)
     {
-        x += hmc5883l.getMx()/100;
-        y += hmc5883l.getMy()/100;
-        z += hmc5883l.getMz()/100;
-        wait_ms(100);
+        case 1: // body
+            Read_Magnetic_body();
+            
+            heading = atan2(y,x); // find the angle
+            printf("heading after atan2: %f\n",heading);
+            heading = (heading * 180 / PI); //convert to degree
+            printf("heading after atan2 (degrees): %f\n",heading);
+            
+            //heading += declinationAngle + (BODY_MOUNT_RAD_FIX);
+            //printf("heading after fixes: %f\n",heading);
+           
+           if (heading >0 )
+             heading -= 90; // sensor rotated by 90 degrees  
+           else if (heading < 0)
+                heading+=90;  
+                
+             //fix for arm sensor mount angle
+            if(heading > 180) heading -= 180;
+            else if(heading < 0) heading += 180; 
+             
+            //---------- map degrees from -180->180 to 0->360 (only positive degrees) ---------- //
+           // if(heading < 0 && heading >= -180) 
+          //          heading += 360;
+            //-----------------------------------------------------------------------------------//
+               
+           
+            
+            ////fix for arm sensor mount angle
+//            if(heading > 360) heading -= 360;
+//            if(heading < 0) heading += 360; 
+            
+            
+            //heading = (heading * 180 / PI); //convert to degree
+             printf("heading final: %f\n\n",heading);
+          //  printf("heading: %f\n", heading);  // debug    
+             
+            break;
+                    
+        case 2: // arm
+            Read_Magnetic_arm();
+            heading = atan2(y,x); // find the angle
+            heading += declinationAngle + Arm_RAD_FIX ;   
+            if(heading < 0) heading += 2*PI;
+            if(heading > 2*PI) heading -= 2*PI;
+            heading = (heading * 180 / PI); //convert to degree               
+//            heading += ARM_MOUNT_ANGLE - TargetAngle;
+            if(heading < 180 ) heading = - heading;
+            else // 180 <= heading <= 360
+                heading = 360 - heading;
+            break;      
     }
     
-
-    m_x = x/2;
-    m_y = y/2;
-    m_z = z/2;
-    */
-    wait(1);
+    
+    return heading;
+}
 
-    while(1) 
-    { 
-        /*
-        x = m_x - hmc5883l.getMx()*0.92;
-        y = m_y - hmc5883l.getMy()*0.92;
-        z = m_z - hmc5883l.getMz()*0.92;
-        */
-        x = hmc5883l.getMx();
-        y = hmc5883l.getMy();
-        z = hmc5883l.getMz();
-        
-        heading = atan2(y, x);
-        if(heading < 0) 
-            heading += 2*PI;
-        if(heading > 2*PI) 
-            heading -= 2*PI;
-        
-        heading = heading * 180 / PI;
-        
-        // Correct for when signs are reversed.
-        //if(heading < 0) 
-        //    heading += 2*PI;
-        //if(heading > 2*PI) 
-        //    heading -= 2*PI;
-       
-        //while(heading < 0) heading += 360;
-        //while(heading > 360) heading -= 360;
-
-
-        printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, heading);
-        wait_ms(200);
-
+int main()
+{
+   
+    BT.baud(115200);
+    Servo.period(0.020); // set PWM period time to 20ms period or 50hz frequency
+  // hmc_arm.Write(0x02,0x00); // set magnetic sensor on arm to continuous mode
+  //  pc.attach(&DataReceived_Pc_ISR,Serial::RxIrq); //maor
+    BT.attach(&DataReceived_ISR,Serial::RxIrq); //maor
+    wait(1);
+    //globalTimer.start();
+    pc.printf("ready");
+    //Thread t1(thread1); //start thread1 ARM
+    //t1.set_priority(osPriorityHigh);
+    
+    Thread t2(thread2); //start thread2 BT SEND
+    t2.set_priority(osPriorityNormal);
+    
+    //Thread t3(thread3); //start thread3 BT RECIEVE
+    //t3.set_priority(osPriorityNormal);
+    
+    Servo.pulsewidth_us(2000);
+    wait(2);
+    Servo.pulsewidth_us(1500);
+    wait(2);
+    Servo.pulsewidth_us(1000);
+    wait(2);
+    Servo.pulsewidth_us(1500);
+    
+    while(1) {       // main is the next thread 
+//    if(pc.readable())
+//    {
+//        int x;
+//        pc.scanf("%d",&x); 
+//        printf("got:%d\n",x);
+//       Servo.pulsewidth_us(x);    
+//    }
+//        
     }
-}
+}
\ No newline at end of file