![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StingerBot Project
Dependencies: mbed HMC6352 mbed-rtos
main.cpp@2:56eb726bdb0d, 2014-04-22 (annotated)
- Committer:
- Strikewolf
- Date:
- Tue Apr 22 23:16:32 2014 +0000
- Revision:
- 2:56eb726bdb0d
- Parent:
- 1:41cee26b35cc
Complete initial working RobotControl drive functions using the honeywell compass module. Also added in emergency stop function.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Strikewolf | 0:b6fd1c37944a | 1 | #include "mbed.h" |
Strikewolf | 0:b6fd1c37944a | 2 | #include "RobotControl.h" |
Strikewolf | 2:56eb726bdb0d | 3 | #include "rtos.h" |
Strikewolf | 2:56eb726bdb0d | 4 | |
Strikewolf | 2:56eb726bdb0d | 5 | #define ENTERCALIB 0x43 |
Strikewolf | 2:56eb726bdb0d | 6 | #define EXITCALIB 0x45 |
Strikewolf | 2:56eb726bdb0d | 7 | |
Strikewolf | 2:56eb726bdb0d | 8 | InterruptIn sw(p30); |
Strikewolf | 2:56eb726bdb0d | 9 | |
Strikewolf | 2:56eb726bdb0d | 10 | void StopISR() |
Strikewolf | 2:56eb726bdb0d | 11 | { |
Strikewolf | 2:56eb726bdb0d | 12 | rightMotorPWM = 0; |
Strikewolf | 2:56eb726bdb0d | 13 | leftMotorPWM = 0; |
Strikewolf | 2:56eb726bdb0d | 14 | exit(1); |
Strikewolf | 2:56eb726bdb0d | 15 | } |
Strikewolf | 2:56eb726bdb0d | 16 | |
Strikewolf | 2:56eb726bdb0d | 17 | void InitCompass() { |
Strikewolf | 2:56eb726bdb0d | 18 | //Init compass |
Strikewolf | 2:56eb726bdb0d | 19 | compass.setReset(); |
Strikewolf | 2:56eb726bdb0d | 20 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
Strikewolf | 2:56eb726bdb0d | 21 | |
Strikewolf | 2:56eb726bdb0d | 22 | //Calibration level - Garrus |
Strikewolf | 2:56eb726bdb0d | 23 | //Rotate robot one full rotation during this section |
Strikewolf | 2:56eb726bdb0d | 24 | pc.printf("Begin Calibration...\r\n"); |
Strikewolf | 2:56eb726bdb0d | 25 | compass.setCalibrationMode(ENTERCALIB); |
Strikewolf | 2:56eb726bdb0d | 26 | wait(10); |
Strikewolf | 2:56eb726bdb0d | 27 | compass.setCalibrationMode(EXITCALIB); |
Strikewolf | 2:56eb726bdb0d | 28 | |
Strikewolf | 2:56eb726bdb0d | 29 | //Sample a few to clean out buffer |
Strikewolf | 2:56eb726bdb0d | 30 | compass.sample(); |
Strikewolf | 2:56eb726bdb0d | 31 | compass.sample(); |
Strikewolf | 2:56eb726bdb0d | 32 | compass.sample(); |
Strikewolf | 2:56eb726bdb0d | 33 | } |
Strikewolf | 0:b6fd1c37944a | 34 | |
Strikewolf | 0:b6fd1c37944a | 35 | int main() { |
Strikewolf | 2:56eb726bdb0d | 36 | //Emergency stop mechanism |
Strikewolf | 2:56eb726bdb0d | 37 | sw.rise(&StopISR); |
Strikewolf | 2:56eb726bdb0d | 38 | |
Strikewolf | 2:56eb726bdb0d | 39 | //Setup mode and perform calibration |
Strikewolf | 2:56eb726bdb0d | 40 | InitCompass(); |
Strikewolf | 2:56eb726bdb0d | 41 | |
Strikewolf | 2:56eb726bdb0d | 42 | compassDriveStraight(0.7, false, 20000); |
Strikewolf | 0:b6fd1c37944a | 43 | |
Strikewolf | 0:b6fd1c37944a | 44 | while(1) { |
Strikewolf | 2:56eb726bdb0d | 45 | float sample = compass.sample() / 10; |
Strikewolf | 2:56eb726bdb0d | 46 | pc.printf("%f\r\n", sample); |
Strikewolf | 0:b6fd1c37944a | 47 | } |
Strikewolf | 0:b6fd1c37944a | 48 | } |