File content as of revision 1:aea254b39529:

#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

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

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

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)
    {
        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;      
    }
    
    
    return heading;
}

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