main.cpp

00001 #include "mbed.h"
00002 #include "HMC5883L.h"
00003 #include <math.h>
00004 #include "rtos.h"
00005 #include <string>
00006 #define SDA_ARM      p28 //p9
00007 #define SCL_ARM      p27 //p10
00008 #define SDA_BODY      p9 //p28
00009 #define SCL_BODY      p10 //p27
00010 #define PI       3.14159265
00011 #define SERVO_PIN p21
00012 #define MaxDegreeDeviation 2 // how many degrees to ignore near north / target
00013 #define ARM_MOUNT_ANGLE -70
00014 #define BODY_MOUNT_ANGLE 0 //was -50
00015 #define CCW_MIN_SPEED 1540  //1550 
00016 #define CCW_MAX_SPEED 1800
00017 
00018 #define CW_MIN_SPEED 1460  // 1450 
00019 #define CW_MAX_SPEED 1200
00020 
00021 //#define DEBUG_ARM_LOCK_TIMER
00022 //#define DEBUG_SERVO_PWM
00023 //#define DEBUG_GRAPH
00024 
00025 Serial pc(USBTX,USBRX);
00026 int TargetAngle = 0;
00027 float BODY_MOUNT_RAD_FIX =   BODY_MOUNT_ANGLE / 180.0 * PI;
00028 float Arm_RAD_FIX = (ARM_MOUNT_ANGLE/180.0*PI) - (TargetAngle/180.0 * PI);
00029 float declinationAngle = 0.069; // fix by position (radian) - netanya is 0.069 (3.6 deg)
00030 PwmOut Servo(SERVO_PIN);
00031 bool moveFlag = false;
00032 
00033 Serial BT(p13, p14);
00034 float degreeToNorthArm = 0,degreeToNorthBody=0;
00035 int bodyLastDegree = 0;
00036 float z,x,y;;
00037 HMC5883L hmc_arm(SDA_ARM, SCL_ARM);
00038 HMC5883L hmc_body(SDA_BODY, SCL_BODY);
00039 
00040 DigitalOut led2(LED2);
00041 
00042 Ticker BTtimer;
00043 Ticker Arm_timer;
00044 Timer ArmlockTimer;
00045 Timer globalTimer;
00046 
00047 //char globalChar = ' ';
00048 string globalString =" ";
00049 
00050 //prototypes:
00051 void BTsendDegrees(float degree);
00052 void BTsendInterval();
00053 void Arm_Ticker();
00054 void Read_Magnetic_arm();
00055 void Read_Magnetic_body();
00056 float GetDegreeToNorth(int sensor);
00057 
00058 
00059 
00060 float map(float x, float in_min, float in_max, float out_min, float out_max)
00061 {
00062     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00063 }
00064 
00065 void RotateClockWise(int moveSpeed)
00066 {
00067     led2 = 1;
00068     moveFlag = true;
00069     int moveSpeedCalc = map(180-moveSpeed,MaxDegreeDeviation,180,CW_MAX_SPEED,CW_MIN_SPEED);
00070     Servo.pulsewidth_us(moveSpeedCalc);
00071    // Servo.pulsewidth(CW_MAX_SPEED);
00072    // wait_ms(SERVO_WAIT_TRIGGERS);
00073     //Servo.pulsewidth_us(1500);
00074     
00075     #ifdef DEBUG_SERVO_PWM
00076     printf("CW  Servo Heading Speed : %d\tActualPwm: %.6f\n",moveSpeed,moveSpeedCalc);
00077     #endif
00078     
00079     #ifdef DEBUG_ARM_LOCK_TIMER
00080     ArmlockTimer.start();
00081     #endif
00082     
00083     #ifdef DEBUG_GRAPH
00084     printf("%d,%d,%d\n",globalTimer.read_ms(),moveSpeed,moveSpeedCalc);
00085     #endif
00086 }
00087 
00088 
00089 void RotateCounterClockWise(int moveSpeed)
00090 {
00091     led2 = 1;
00092     moveFlag = true;
00093     int moveSpeedCalc = map(abs(moveSpeed),MaxDegreeDeviation,180,CCW_MIN_SPEED,CCW_MAX_SPEED);
00094     Servo.pulsewidth_us(moveSpeedCalc);
00095    // Servo.pulsewidth(CCW_MAX_SPEED);
00096    // wait_ms(SERVO_WAIT_TRIGGERS);
00097     //Servo.pulsewidth_us(1500);
00098     #ifdef DEBUG_SERVO_PWM
00099     printf("CCW Servo Heading Speed : %d\tActualPwm: %.6f\n",moveSpeed,moveSpeedCalc);
00100     #endif
00101     #ifdef DEBUG_ARM_LOCK_TIMER
00102     ArmlockTimer.start();
00103     #endif
00104     #ifdef DEBUG_GRAPH
00105     printf("%d,%d,%d\n",globalTimer.read_ms(),moveSpeed,moveSpeedCalc);
00106     #endif
00107 }
00108 
00109 void StopServo()
00110 {
00111     led2 = 0;
00112     //myServo.writeMicroseconds(1500);
00113     Servo.pulsewidth(0.0015); // pulse width 1500ms - servo stop
00114     #ifdef DEBUG_ARM_LOCK_TIMER
00115     if(moveFlag)
00116     {
00117         ArmlockTimer.stop();     
00118      //   printf("Arm locking time : %d ms\n",ArmlockTimer.read_ms());  // - Print the incoming command
00119         ArmlockTimer.reset();
00120         moveFlag = false;       
00121     }
00122     #endif  
00123 }
00124 
00125 
00126 void DataReceived_ISR(){
00127      char globalChar = LPC_UART1->RBR;
00128      globalString += globalChar; 
00129 }
00130 
00131 int getCommandValue(string cmd){
00132     string cmdValue;   
00133     std::size_t sharpPosition = cmd.find("#");  
00134     cmdValue = cmd.substr(sharpPosition+1);
00135  //   printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
00136     return atoi(cmdValue.c_str());
00137 }
00138 
00139 void BTCommands (string cmd)
00140 {
00141    
00142    //printf("Input = %s\n",globalString);  // - Print the incoming command
00143     
00144    if (cmd.find("Target")!= string::npos)
00145    {
00146        TargetAngle = getCommandValue(cmd);
00147        Arm_RAD_FIX = Arm_RAD_FIX = (ARM_MOUNT_ANGLE/180.0*PI) - (TargetAngle/180.0 * PI);
00148      //  printf("New Target Angle set to: %d\n",TargetAngle);
00149    }
00150       
00151 }
00152 
00153 void thread1(void const *args) //arm thread
00154 {
00155     while(true) {
00156         degreeToNorthArm = GetDegreeToNorth(2); // 2 is arm sensor
00157 
00158         if(degreeToNorthArm > MaxDegreeDeviation)
00159             RotateClockWise(degreeToNorthArm);                   
00160         else if(degreeToNorthArm < -MaxDegreeDeviation)
00161             RotateCounterClockWise(degreeToNorthArm);
00162                       
00163         else  
00164           StopServo();  
00165                          
00166         Thread::wait(10);
00167     }
00168 }
00169 
00170 void thread2(void const *args) //body thread
00171 {
00172     while(true) { 
00173     degreeToNorthBody = GetDegreeToNorth(1);
00174     if(abs(bodyLastDegree - degreeToNorthBody) > 2)
00175     {     
00176        // printf("degrees: %f\n ",degreeToNorthBody);
00177         BTsendDegrees(degreeToNorthBody);
00178         bodyLastDegree = degreeToNorthBody;
00179         Thread::wait(800);     
00180     }
00181     else  
00182         Thread::wait(800);
00183     }
00184 }
00185 
00186 
00187 
00188 void thread3(void const *args)
00189 {
00190     while(true) { 
00191         if(globalString.length() > 0 )
00192         {
00193             BTCommands(globalString);
00194             globalString = "";
00195         }
00196         
00197         Thread::wait(100);
00198     }
00199 }
00200 
00201 void BTsendDegrees(float degree)
00202 {
00203       BT.printf("#%f~",degree);
00204 }
00205 
00206 void BTsendInterval()
00207 {
00208     degreeToNorthBody = GetDegreeToNorth(1);
00209     BTsendDegrees(degreeToNorthBody);
00210    // Arm_timer.attach(Arm_Ticker, 0.0001); // activate arm sensor read frequency
00211 }
00212 
00213 void Read_Magnetic_arm()
00214 {
00215     x = hmc_arm.getMx();
00216     y = hmc_arm.getMy();
00217     z = hmc_arm.getMz();
00218 }
00219 
00220 void Read_Magnetic_body()
00221 {
00222     x = hmc_body.getMx();
00223     y = hmc_body.getMy();
00224     z = hmc_body.getMz();
00225 }
00226 
00227 float GetDegreeToNorth(int sensor)
00228 {
00229     float heading;
00230      
00231        
00232     switch(sensor)
00233     {
00234         case 1: // body
00235             Read_Magnetic_body();
00236             
00237             heading = atan2(y,x); // find the angle
00238             printf("heading after atan2: %f\n",heading);
00239             heading = (heading * 180 / PI); //convert to degree
00240             printf("heading after atan2 (degrees): %f\n",heading);
00241             
00242             //heading += declinationAngle + (BODY_MOUNT_RAD_FIX);
00243             //printf("heading after fixes: %f\n",heading);
00244            
00245            if (heading >0 )
00246              heading -= 90; // sensor rotated by 90 degrees  
00247            else if (heading < 0)
00248                 heading+=90;  
00249                 
00250              //fix for arm sensor mount angle
00251             if(heading > 180) heading -= 180;
00252             else if(heading < 0) heading += 180; 
00253              
00254             //---------- map degrees from -180->180 to 0->360 (only positive degrees) ---------- //
00255            // if(heading < 0 && heading >= -180) 
00256           //          heading += 360;
00257             //-----------------------------------------------------------------------------------//
00258                
00259            
00260             
00261             ////fix for arm sensor mount angle
00262 //            if(heading > 360) heading -= 360;
00263 //            if(heading < 0) heading += 360; 
00264             
00265             
00266             //heading = (heading * 180 / PI); //convert to degree
00267              printf("heading final: %f\n\n",heading);
00268           //  printf("heading: %f\n", heading);  // debug    
00269              
00270             break;
00271                     
00272         case 2: // arm
00273             Read_Magnetic_arm();
00274             heading = atan2(y,x); // find the angle
00275             heading += declinationAngle + Arm_RAD_FIX ;   
00276             if(heading < 0) heading += 2*PI;
00277             if(heading > 2*PI) heading -= 2*PI;
00278             heading = (heading * 180 / PI); //convert to degree               
00279 //            heading += ARM_MOUNT_ANGLE - TargetAngle;
00280             if(heading < 180 ) heading = - heading;
00281             else // 180 <= heading <= 360
00282                 heading = 360 - heading;
00283             break;      
00284     }
00285     
00286     
00287     return heading;
00288 }
00289 
00290 int main()
00291 {
00292    
00293     BT.baud(115200);
00294     Servo.period(0.020); // set PWM period time to 20ms period or 50hz frequency
00295   // hmc_arm.Write(0x02,0x00); // set magnetic sensor on arm to continuous mode
00296   //  pc.attach(&DataReceived_Pc_ISR,Serial::RxIrq); //maor
00297     BT.attach(&DataReceived_ISR,Serial::RxIrq); //maor
00298     wait(1);
00299     //globalTimer.start();
00300     pc.printf("ready");
00301     //Thread t1(thread1); //start thread1 ARM
00302     //t1.set_priority(osPriorityHigh);
00303     
00304     Thread t2(thread2); //start thread2 BT SEND
00305     t2.set_priority(osPriorityNormal);
00306     
00307     //Thread t3(thread3); //start thread3 BT RECIEVE
00308     //t3.set_priority(osPriorityNormal);
00309     
00310     Servo.pulsewidth_us(2000);
00311     wait(2);
00312     Servo.pulsewidth_us(1500);
00313     wait(2);
00314     Servo.pulsewidth_us(1000);
00315     wait(2);
00316     Servo.pulsewidth_us(1500);
00317     
00318     while(1) {       // main is the next thread 
00319 //    if(pc.readable())
00320 //    {
00321 //        int x;
00322 //        pc.scanf("%d",&x); 
00323 //        printf("got:%d\n",x);
00324 //       Servo.pulsewidth_us(x);    
00325 //    }
00326 //        
00327     }
00328 }