Example of reading and magnetometer sensor (HMC5883L)
Dependencies: MODSERIAL mbed-rtos mbed
Fork of ReadingMag_HMC5883L by
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?
User | Revision | Line number | New 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 | } |