Justin Eng / Mbed 2 deprecated Project

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?

UserRevisionLine numberNew 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 }