Example of reading and magnetometer sensor (HMC5883L)
Dependencies: MODSERIAL mbed-rtos mbed
Fork of ReadingMag_HMC5883L by
main.cpp
- Committer:
- Maor_T
- Date:
- 2016-05-24
- Revision:
- 1:aea254b39529
- Parent:
- 0:6bc5f85ca6fa
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); // } // } }