Maor Tanami / Mbed 2 deprecated Final_Project

Example of reading and magnetometer sensor (HMC5883L)

Dependencies:   MODSERIAL mbed-rtos mbed

Fork of ReadingMag_HMC5883L by José Claudio

main.cpp@1:aea254b39529, 2016-05-24 (annotated)

Committer:
Maor_T
Date:
Tue May 24 12:08:14 2016 +0000
Revision:
1:aea254b39529
Parent:
0:6bc5f85ca6fa 
amir

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jose_claudiojr 0:6bc5f85ca6fa 1 #include "mbed.h"
jose_claudiojr 0:6bc5f85ca6fa 2 #include "HMC5883L.h"
Maor_T 1:aea254b39529 3 #include <math.h>
Maor_T 1:aea254b39529 4 #include "rtos.h"
Maor_T 1:aea254b39529 5 #include <string>
Maor_T 1:aea254b39529 6 #define SDA_ARM p28 //p9
Maor_T 1:aea254b39529 7 #define SCL_ARM p27 //p10
Maor_T 1:aea254b39529 8 #define SDA_BODY p9 //p28
Maor_T 1:aea254b39529 9 #define SCL_BODY p10 //p27
Maor_T 1:aea254b39529 10 #define PI 3.14159265
Maor_T 1:aea254b39529 11 #define SERVO_PIN p21
Maor_T 1:aea254b39529 12 #define MaxDegreeDeviation 2 // how many degrees to ignore near north / target
Maor_T 1:aea254b39529 13 #define ARM_MOUNT_ANGLE -70
Maor_T 1:aea254b39529 14 #define BODY_MOUNT_ANGLE 0 //was -50
Maor_T 1:aea254b39529 15 #define CCW_MIN_SPEED 1540 //1550
Maor_T 1:aea254b39529 16 #define CCW_MAX_SPEED 1800
jose_claudiojr 0:6bc5f85ca6fa 17
Maor_T 1:aea254b39529 18 #define CW_MIN_SPEED 1460 // 1450
Maor_T 1:aea254b39529 19 #define CW_MAX_SPEED 1200
Maor_T 1:aea254b39529 20
Maor_T 1:aea254b39529 21 //#define DEBUG_ARM_LOCK_TIMER
Maor_T 1:aea254b39529 22 //#define DEBUG_SERVO_PWM
Maor_T 1:aea254b39529 23 //#define DEBUG_GRAPH
Maor_T 1:aea254b39529 24
Maor_T 1:aea254b39529 25 Serial pc(USBTX,USBRX);
Maor_T 1:aea254b39529 26 int TargetAngle = 0;
Maor_T 1:aea254b39529 27 float BODY_MOUNT_RAD_FIX = BODY_MOUNT_ANGLE / 180.0 * PI;
Maor_T 1:aea254b39529 28 float Arm_RAD_FIX = (ARM_MOUNT_ANGLE/180.0*PI) - (TargetAngle/180.0 * PI);
Maor_T 1:aea254b39529 29 float declinationAngle = 0.069; // fix by position (radian) - netanya is 0.069 (3.6 deg)
Maor_T 1:aea254b39529 30 PwmOut Servo(SERVO_PIN);
Maor_T 1:aea254b39529 31 bool moveFlag = false;
Maor_T 1:aea254b39529 32
Maor_T 1:aea254b39529 33 Serial BT(p13, p14);
Maor_T 1:aea254b39529 34 float degreeToNorthArm = 0,degreeToNorthBody=0;
Maor_T 1:aea254b39529 35 int bodyLastDegree = 0;
Maor_T 1:aea254b39529 36 float z,x,y;;
Maor_T 1:aea254b39529 37 HMC5883L hmc_arm(SDA_ARM, SCL_ARM);
Maor_T 1:aea254b39529 38 HMC5883L hmc_body(SDA_BODY, SCL_BODY);
Maor_T 1:aea254b39529 39
Maor_T 1:aea254b39529 40 DigitalOut led2(LED2);
Maor_T 1:aea254b39529 41
Maor_T 1:aea254b39529 42 Ticker BTtimer;
Maor_T 1:aea254b39529 43 Ticker Arm_timer;
Maor_T 1:aea254b39529 44 Timer ArmlockTimer;
Maor_T 1:aea254b39529 45 Timer globalTimer;
Maor_T 1:aea254b39529 46
Maor_T 1:aea254b39529 47 //char globalChar = ' ';
Maor_T 1:aea254b39529 48 string globalString =" ";
Maor_T 1:aea254b39529 49
Maor_T 1:aea254b39529 50 //prototypes:
Maor_T 1:aea254b39529 51 void BTsendDegrees(float degree);
Maor_T 1:aea254b39529 52 void BTsendInterval();
Maor_T 1:aea254b39529 53 void Arm_Ticker();
Maor_T 1:aea254b39529 54 void Read_Magnetic_arm();
Maor_T 1:aea254b39529 55 void Read_Magnetic_body();
Maor_T 1:aea254b39529 56 float GetDegreeToNorth(int sensor);
Maor_T 1:aea254b39529 57
Maor_T 1:aea254b39529 58
Maor_T 1:aea254b39529 59
Maor_T 1:aea254b39529 60 float map(float x, float in_min, float in_max, float out_min, float out_max)
Maor_T 1:aea254b39529 61 {
Maor_T 1:aea254b39529 62 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
Maor_T 1:aea254b39529 63 }
jose_claudiojr 0:6bc5f85ca6fa 64
Maor_T 1:aea254b39529 65 void RotateClockWise(int moveSpeed)
Maor_T 1:aea254b39529 66 {
Maor_T 1:aea254b39529 67 led2 = 1;
Maor_T 1:aea254b39529 68 moveFlag = true;
Maor_T 1:aea254b39529 69 int moveSpeedCalc = map(180-moveSpeed,MaxDegreeDeviation,180,CW_MAX_SPEED,CW_MIN_SPEED);
Maor_T 1:aea254b39529 70 Servo.pulsewidth_us(moveSpeedCalc);
Maor_T 1:aea254b39529 71 // Servo.pulsewidth(CW_MAX_SPEED);
Maor_T 1:aea254b39529 72 // wait_ms(SERVO_WAIT_TRIGGERS);
Maor_T 1:aea254b39529 73 //Servo.pulsewidth_us(1500);
Maor_T 1:aea254b39529 74
Maor_T 1:aea254b39529 75 #ifdef DEBUG_SERVO_PWM
Maor_T 1:aea254b39529 76 printf("CW Servo Heading Speed : %d\tActualPwm: %.6f\n",moveSpeed,moveSpeedCalc);
Maor_T 1:aea254b39529 77 #endif
Maor_T 1:aea254b39529 78
Maor_T 1:aea254b39529 79 #ifdef DEBUG_ARM_LOCK_TIMER
Maor_T 1:aea254b39529 80 ArmlockTimer.start();
Maor_T 1:aea254b39529 81 #endif
Maor_T 1:aea254b39529 82
Maor_T 1:aea254b39529 83 #ifdef DEBUG_GRAPH
Maor_T 1:aea254b39529 84 printf("%d,%d,%d\n",globalTimer.read_ms(),moveSpeed,moveSpeedCalc);
Maor_T 1:aea254b39529 85 #endif
Maor_T 1:aea254b39529 86 }
Maor_T 1:aea254b39529 87
Maor_T 1:aea254b39529 88
Maor_T 1:aea254b39529 89 void RotateCounterClockWise(int moveSpeed)
Maor_T 1:aea254b39529 90 {
Maor_T 1:aea254b39529 91 led2 = 1;
Maor_T 1:aea254b39529 92 moveFlag = true;
Maor_T 1:aea254b39529 93 int moveSpeedCalc = map(abs(moveSpeed),MaxDegreeDeviation,180,CCW_MIN_SPEED,CCW_MAX_SPEED);
Maor_T 1:aea254b39529 94 Servo.pulsewidth_us(moveSpeedCalc);
Maor_T 1:aea254b39529 95 // Servo.pulsewidth(CCW_MAX_SPEED);
Maor_T 1:aea254b39529 96 // wait_ms(SERVO_WAIT_TRIGGERS);
Maor_T 1:aea254b39529 97 //Servo.pulsewidth_us(1500);
Maor_T 1:aea254b39529 98 #ifdef DEBUG_SERVO_PWM
Maor_T 1:aea254b39529 99 printf("CCW Servo Heading Speed : %d\tActualPwm: %.6f\n",moveSpeed,moveSpeedCalc);
Maor_T 1:aea254b39529 100 #endif
Maor_T 1:aea254b39529 101 #ifdef DEBUG_ARM_LOCK_TIMER
Maor_T 1:aea254b39529 102 ArmlockTimer.start();
Maor_T 1:aea254b39529 103 #endif
Maor_T 1:aea254b39529 104 #ifdef DEBUG_GRAPH
Maor_T 1:aea254b39529 105 printf("%d,%d,%d\n",globalTimer.read_ms(),moveSpeed,moveSpeedCalc);
Maor_T 1:aea254b39529 106 #endif
Maor_T 1:aea254b39529 107 }
Maor_T 1:aea254b39529 108
Maor_T 1:aea254b39529 109 void StopServo()
Maor_T 1:aea254b39529 110 {
Maor_T 1:aea254b39529 111 led2 = 0;
Maor_T 1:aea254b39529 112 //myServo.writeMicroseconds(1500);
Maor_T 1:aea254b39529 113 Servo.pulsewidth(0.0015); // pulse width 1500ms - servo stop
Maor_T 1:aea254b39529 114 #ifdef DEBUG_ARM_LOCK_TIMER
Maor_T 1:aea254b39529 115 if(moveFlag)
Maor_T 1:aea254b39529 116 {
Maor_T 1:aea254b39529 117 ArmlockTimer.stop();
Maor_T 1:aea254b39529 118 // printf("Arm locking time : %d ms\n",ArmlockTimer.read_ms()); // - Print the incoming command
Maor_T 1:aea254b39529 119 ArmlockTimer.reset();
Maor_T 1:aea254b39529 120 moveFlag = false;
Maor_T 1:aea254b39529 121 }
Maor_T 1:aea254b39529 122 #endif
Maor_T 1:aea254b39529 123 }
Maor_T 1:aea254b39529 124
Maor_T 1:aea254b39529 125
Maor_T 1:aea254b39529 126 void DataReceived_ISR(){
Maor_T 1:aea254b39529 127 char globalChar = LPC_UART1->RBR;
Maor_T 1:aea254b39529 128 globalString += globalChar;
Maor_T 1:aea254b39529 129 }
Maor_T 1:aea254b39529 130
Maor_T 1:aea254b39529 131 int getCommandValue(string cmd){
Maor_T 1:aea254b39529 132 string cmdValue;
Maor_T 1:aea254b39529 133 std::size_t sharpPosition = cmd.find("#");
Maor_T 1:aea254b39529 134 cmdValue = cmd.substr(sharpPosition+1);
Maor_T 1:aea254b39529 135 // printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
Maor_T 1:aea254b39529 136 return atoi(cmdValue.c_str());
Maor_T 1:aea254b39529 137 }
jose_claudiojr 0:6bc5f85ca6fa 138
Maor_T 1:aea254b39529 139 void BTCommands (string cmd)
Maor_T 1:aea254b39529 140 {
Maor_T 1:aea254b39529 141
Maor_T 1:aea254b39529 142 //printf("Input = %s\n",globalString); // - Print the incoming command
Maor_T 1:aea254b39529 143
Maor_T 1:aea254b39529 144 if (cmd.find("Target")!= string::npos)
Maor_T 1:aea254b39529 145 {
Maor_T 1:aea254b39529 146 TargetAngle = getCommandValue(cmd);
Maor_T 1:aea254b39529 147 Arm_RAD_FIX = Arm_RAD_FIX = (ARM_MOUNT_ANGLE/180.0*PI) - (TargetAngle/180.0 * PI);
Maor_T 1:aea254b39529 148 // printf("New Target Angle set to: %d\n",TargetAngle);
Maor_T 1:aea254b39529 149 }
Maor_T 1:aea254b39529 150
Maor_T 1:aea254b39529 151 }
Maor_T 1:aea254b39529 152
Maor_T 1:aea254b39529 153 void thread1(void const *args) //arm thread
Maor_T 1:aea254b39529 154 {
Maor_T 1:aea254b39529 155 while(true) {
Maor_T 1:aea254b39529 156 degreeToNorthArm = GetDegreeToNorth(2); // 2 is arm sensor
Maor_T 1:aea254b39529 157
Maor_T 1:aea254b39529 158 if(degreeToNorthArm > MaxDegreeDeviation)
Maor_T 1:aea254b39529 159 RotateClockWise(degreeToNorthArm);
Maor_T 1:aea254b39529 160 else if(degreeToNorthArm < -MaxDegreeDeviation)
Maor_T 1:aea254b39529 161 RotateCounterClockWise(degreeToNorthArm);
Maor_T 1:aea254b39529 162
Maor_T 1:aea254b39529 163 else
Maor_T 1:aea254b39529 164 StopServo();
Maor_T 1:aea254b39529 165
Maor_T 1:aea254b39529 166 Thread::wait(10);
Maor_T 1:aea254b39529 167 }
Maor_T 1:aea254b39529 168 }
Maor_T 1:aea254b39529 169
Maor_T 1:aea254b39529 170 void thread2(void const *args) //body thread
Maor_T 1:aea254b39529 171 {
Maor_T 1:aea254b39529 172 while(true) {
Maor_T 1:aea254b39529 173 degreeToNorthBody = GetDegreeToNorth(1);
Maor_T 1:aea254b39529 174 if(abs(bodyLastDegree - degreeToNorthBody) > 2)
Maor_T 1:aea254b39529 175 {
Maor_T 1:aea254b39529 176 // printf("degrees: %f\n ",degreeToNorthBody);
Maor_T 1:aea254b39529 177 BTsendDegrees(degreeToNorthBody);
Maor_T 1:aea254b39529 178 bodyLastDegree = degreeToNorthBody;
Maor_T 1:aea254b39529 179 Thread::wait(800);
Maor_T 1:aea254b39529 180 }
Maor_T 1:aea254b39529 181 else
Maor_T 1:aea254b39529 182 Thread::wait(800);
Maor_T 1:aea254b39529 183 }
Maor_T 1:aea254b39529 184 }
Maor_T 1:aea254b39529 185
Maor_T 1:aea254b39529 186
Maor_T 1:aea254b39529 187
Maor_T 1:aea254b39529 188 void thread3(void const *args)
Maor_T 1:aea254b39529 189 {
Maor_T 1:aea254b39529 190 while(true) {
Maor_T 1:aea254b39529 191 if(globalString.length() > 0 )
Maor_T 1:aea254b39529 192 {
Maor_T 1:aea254b39529 193 BTCommands(globalString);
Maor_T 1:aea254b39529 194 globalString = "";
Maor_T 1:aea254b39529 195 }
Maor_T 1:aea254b39529 196
Maor_T 1:aea254b39529 197 Thread::wait(100);
Maor_T 1:aea254b39529 198 }
Maor_T 1:aea254b39529 199 }
Maor_T 1:aea254b39529 200
Maor_T 1:aea254b39529 201 void BTsendDegrees(float degree)
Maor_T 1:aea254b39529 202 {
Maor_T 1:aea254b39529 203 BT.printf("#%f~",degree);
Maor_T 1:aea254b39529 204 }
Maor_T 1:aea254b39529 205
Maor_T 1:aea254b39529 206 void BTsendInterval()
Maor_T 1:aea254b39529 207 {
Maor_T 1:aea254b39529 208 degreeToNorthBody = GetDegreeToNorth(1);
Maor_T 1:aea254b39529 209 BTsendDegrees(degreeToNorthBody);
Maor_T 1:aea254b39529 210 // Arm_timer.attach(Arm_Ticker, 0.0001); // activate arm sensor read frequency
Maor_T 1:aea254b39529 211 }
Maor_T 1:aea254b39529 212
Maor_T 1:aea254b39529 213 void Read_Magnetic_arm()
Maor_T 1:aea254b39529 214 {
Maor_T 1:aea254b39529 215 x = hmc_arm.getMx();
Maor_T 1:aea254b39529 216 y = hmc_arm.getMy();
Maor_T 1:aea254b39529 217 z = hmc_arm.getMz();
Maor_T 1:aea254b39529 218 }
Maor_T 1:aea254b39529 219
Maor_T 1:aea254b39529 220 void Read_Magnetic_body()
Maor_T 1:aea254b39529 221 {
Maor_T 1:aea254b39529 222 x = hmc_body.getMx();
Maor_T 1:aea254b39529 223 y = hmc_body.getMy();
Maor_T 1:aea254b39529 224 z = hmc_body.getMz();
Maor_T 1:aea254b39529 225 }
Maor_T 1:aea254b39529 226
Maor_T 1:aea254b39529 227 float GetDegreeToNorth(int sensor)
Maor_T 1:aea254b39529 228 {
Maor_T 1:aea254b39529 229 float heading;
Maor_T 1:aea254b39529 230
Maor_T 1:aea254b39529 231
Maor_T 1:aea254b39529 232 switch(sensor)
jose_claudiojr 0:6bc5f85ca6fa 233 {
Maor_T 1:aea254b39529 234 case 1: // body
Maor_T 1:aea254b39529 235 Read_Magnetic_body();
Maor_T 1:aea254b39529 236
Maor_T 1:aea254b39529 237 heading = atan2(y,x); // find the angle
Maor_T 1:aea254b39529 238 printf("heading after atan2: %f\n",heading);
Maor_T 1:aea254b39529 239 heading = (heading * 180 / PI); //convert to degree
Maor_T 1:aea254b39529 240 printf("heading after atan2 (degrees): %f\n",heading);
Maor_T 1:aea254b39529 241
Maor_T 1:aea254b39529 242 //heading += declinationAngle + (BODY_MOUNT_RAD_FIX);
Maor_T 1:aea254b39529 243 //printf("heading after fixes: %f\n",heading);
Maor_T 1:aea254b39529 244
Maor_T 1:aea254b39529 245 if (heading >0 )
Maor_T 1:aea254b39529 246 heading -= 90; // sensor rotated by 90 degrees
Maor_T 1:aea254b39529 247 else if (heading < 0)
Maor_T 1:aea254b39529 248 heading+=90;
Maor_T 1:aea254b39529 249
Maor_T 1:aea254b39529 250 //fix for arm sensor mount angle
Maor_T 1:aea254b39529 251 if(heading > 180) heading -= 180;
Maor_T 1:aea254b39529 252 else if(heading < 0) heading += 180;
Maor_T 1:aea254b39529 253
Maor_T 1:aea254b39529 254 //---------- map degrees from -180->180 to 0->360 (only positive degrees) ---------- //
Maor_T 1:aea254b39529 255 // if(heading < 0 && heading >= -180)
Maor_T 1:aea254b39529 256 // heading += 360;
Maor_T 1:aea254b39529 257 //-----------------------------------------------------------------------------------//
Maor_T 1:aea254b39529 258
Maor_T 1:aea254b39529 259
Maor_T 1:aea254b39529 260
Maor_T 1:aea254b39529 261 ////fix for arm sensor mount angle
Maor_T 1:aea254b39529 262 // if(heading > 360) heading -= 360;
Maor_T 1:aea254b39529 263 // if(heading < 0) heading += 360;
Maor_T 1:aea254b39529 264
Maor_T 1:aea254b39529 265
Maor_T 1:aea254b39529 266 //heading = (heading * 180 / PI); //convert to degree
Maor_T 1:aea254b39529 267 printf("heading final: %f\n\n",heading);
Maor_T 1:aea254b39529 268 // printf("heading: %f\n", heading); // debug
Maor_T 1:aea254b39529 269
Maor_T 1:aea254b39529 270 break;
Maor_T 1:aea254b39529 271
Maor_T 1:aea254b39529 272 case 2: // arm
Maor_T 1:aea254b39529 273 Read_Magnetic_arm();
Maor_T 1:aea254b39529 274 heading = atan2(y,x); // find the angle
Maor_T 1:aea254b39529 275 heading += declinationAngle + Arm_RAD_FIX ;
Maor_T 1:aea254b39529 276 if(heading < 0) heading += 2*PI;
Maor_T 1:aea254b39529 277 if(heading > 2*PI) heading -= 2*PI;
Maor_T 1:aea254b39529 278 heading = (heading * 180 / PI); //convert to degree
Maor_T 1:aea254b39529 279 // heading += ARM_MOUNT_ANGLE - TargetAngle;
Maor_T 1:aea254b39529 280 if(heading < 180 ) heading = - heading;
Maor_T 1:aea254b39529 281 else // 180 <= heading <= 360
Maor_T 1:aea254b39529 282 heading = 360 - heading;
Maor_T 1:aea254b39529 283 break;
jose_claudiojr 0:6bc5f85ca6fa 284 }
jose_claudiojr 0:6bc5f85ca6fa 285
Maor_T 1:aea254b39529 286
Maor_T 1:aea254b39529 287 return heading;
Maor_T 1:aea254b39529 288 }
jose_claudiojr 0:6bc5f85ca6fa 289
Maor_T 1:aea254b39529 290 int main()
Maor_T 1:aea254b39529 291 {
Maor_T 1:aea254b39529 292
Maor_T 1:aea254b39529 293 BT.baud(115200);
Maor_T 1:aea254b39529 294 Servo.period(0.020); // set PWM period time to 20ms period or 50hz frequency
Maor_T 1:aea254b39529 295 // hmc_arm.Write(0x02,0x00); // set magnetic sensor on arm to continuous mode
Maor_T 1:aea254b39529 296 // pc.attach(&DataReceived_Pc_ISR,Serial::RxIrq); //maor
Maor_T 1:aea254b39529 297 BT.attach(&DataReceived_ISR,Serial::RxIrq); //maor
Maor_T 1:aea254b39529 298 wait(1);
Maor_T 1:aea254b39529 299 //globalTimer.start();
Maor_T 1:aea254b39529 300 pc.printf("ready");
Maor_T 1:aea254b39529 301 //Thread t1(thread1); //start thread1 ARM
Maor_T 1:aea254b39529 302 //t1.set_priority(osPriorityHigh);
Maor_T 1:aea254b39529 303
Maor_T 1:aea254b39529 304 Thread t2(thread2); //start thread2 BT SEND
Maor_T 1:aea254b39529 305 t2.set_priority(osPriorityNormal);
Maor_T 1:aea254b39529 306
Maor_T 1:aea254b39529 307 //Thread t3(thread3); //start thread3 BT RECIEVE
Maor_T 1:aea254b39529 308 //t3.set_priority(osPriorityNormal);
Maor_T 1:aea254b39529 309
Maor_T 1:aea254b39529 310 Servo.pulsewidth_us(2000);
Maor_T 1:aea254b39529 311 wait(2);
Maor_T 1:aea254b39529 312 Servo.pulsewidth_us(1500);
Maor_T 1:aea254b39529 313 wait(2);
Maor_T 1:aea254b39529 314 Servo.pulsewidth_us(1000);
Maor_T 1:aea254b39529 315 wait(2);
Maor_T 1:aea254b39529 316 Servo.pulsewidth_us(1500);
Maor_T 1:aea254b39529 317
Maor_T 1:aea254b39529 318 while(1) { // main is the next thread
Maor_T 1:aea254b39529 319 // if(pc.readable())
Maor_T 1:aea254b39529 320 // {
Maor_T 1:aea254b39529 321 // int x;
Maor_T 1:aea254b39529 322 // pc.scanf("%d",&x);
Maor_T 1:aea254b39529 323 // printf("got:%d\n",x);
Maor_T 1:aea254b39529 324 // Servo.pulsewidth_us(x);
Maor_T 1:aea254b39529 325 // }
Maor_T 1:aea254b39529 326 //
jose_claudiojr 0:6bc5f85ca6fa 327 }
Maor_T 1:aea254b39529 328 }