Diff: RobotControl.h

Revision:

5:210cd333f770

Parent:

4:b2a30c313297

Child:

6:3fb9f96765f6

--- a/RobotControl.h	Fri Apr 25 23:08:01 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,167 +0,0 @@
-//*******************************************
-//* Robot motor control and drive functions *
-//*******************************************
- 
-#include "HMC6352.h"
-#include "PositionSystem.h"
-#include "SonarSystem.h"
-#include <math.h>
-
-#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms
- 
-//Radio Calibration Signals - Need to implement calibration routine...
-#define CHAN3_MAX 2060  //Throttle Hi
-#define CHAN3_MIN 1150  //Throttle Lo
-#define CHAN2_MAX 1260  //Elevator Hi
-#define CHAN2_MIN 2290  //Elevator Lo
-#define CHAN1_MAX 2160  //Rudder Hi
-#define CHAN1_MIN 1210  //Rudder Lo
- 
-//Gyro
-HMC6352 compass(p28, p27);
- 
-//H-bridge
-PwmOut rightMotorPWM(p22);  //Channel A
-PwmOut leftMotorPWM(p21);   //Channel B
-DigitalOut leftMotor1(p23);
-DigitalOut leftMotor2(p24);
-DigitalOut rightMotor1(p25);
-DigitalOut rightMotor2(p26);
-
-float sampleCompass(int sample_size) {
-    float c = 0;
-    for(int i = 0; i < sample_size; i++)
-        c += compass.sample() / 10.0;
-    c = c / (double) sample_size;
-    return c;
-}
- 
-//Non-feedback corrected 'dumb' driving
-void setMove(float speed, bool reverse) {
-    //Set speed
-    rightMotorPWM = speed;
-    leftMotorPWM = speed;
-    
-    //Set motor function
-    leftMotor1 = (!reverse) ? 0 : 1;
-    leftMotor2 = (!reverse) ? 1 : 0;
-    rightMotor1 = (!reverse) ? 0 : 1;
-    rightMotor2 = (!reverse) ? 1 : 0;
-}
- 
-void setTurnLeft(float speed) {
-    //Set speed
-    rightMotorPWM = speed;
-    leftMotorPWM = speed;
-    
-    //Set motor function
-    leftMotor1 = 0;
-    leftMotor2 = 1;
-    rightMotor1 = 1;
-    rightMotor2 = 0;
-}
- 
-void setTurnRight(float speed) {
-    //Set speed
-    rightMotorPWM = speed;
-    leftMotorPWM = speed;
-    
-    //Set motor function
-    leftMotor1 = 1;
-    leftMotor2 = 0;
-    rightMotor1 = 0;
-    rightMotor2 = 1;
-}
- 
-//Stop with braking
-void stop() {
-    rightMotorPWM = 0;
-    leftMotorPWM = 0;
-    leftMotor1 = 0;
-    leftMotor2 = 1;
-    rightMotor1 = 1;
-    rightMotor2 = 0;
-}
-
-// calibrate 90 degree turns
-void turnCalibrate() {
-    
-    // tell positioning system to set the turn
-    // calibration variable, PTURN_RIGHT
-    PFlag = PFLAG_CALIB;
-        
-    // start turning hardcoded speed PTURN_SPEED
-    setTurnRight(PTURN_SPEED);
-    
-    // wait until ROTATE_90_TIME has passed. This should
-    // be the time to complete a 90 degree turn
-    wait_ms(ROTATE_90_TIME);
-    stop();
-    
-    // done calibrating, turn positioning system back on
-    PFlag = PFLAG_ON;
-}
-
-// -------------------------------------------------------------------
- 
-//Turn right about the center
-void turnRight(int delta_degrees)
-{
-    pc.printf("Turnleft: \n\r");
-    
-    // turn positioning system off during turns since
-    // no x and y movements
-    PFlag = PFLAG_OFF;
-    
-    // store new orientation after this turn executes
-    t_position += 90;
-    t_position %= 360;   
-    
-    // initialize turn variables
-    turnInit();
-    
-    // start turning right
-    setTurnRight(PTURN_SPEED);
-    
-    // read mouse sensor until 90 degress has completed
-    while(pturn_x > PTURN_RIGHT) {
-        pc.printf("%f, %f\n\r", pturn_x, pturn_y);
-    }
-    
-    // stop turning
-    stop();
-    
-    // turn positioning system back on
-    PFlag = PFLAG_ON;
-}
-
-//Drive straight with compass correction
-void move(float speed, bool reverse) {
-
-    // begin moving
-    setMove(speed, reverse);
-    
-    // blocking call
-    while(1) {
-  
-        // read all sonar sensors
-        float s1 = sonar1.read() * 100;
-        float s2 = sonar2.read() * 100;
-        float s3 = sonar3.read() * 100;
-            
-        // check if obstacle is near and adjust 
-        if(s2 < SONAR_STOP) {
-            pc.printf("%.2f, %.2f, %.2f\n\r", s1, s2, s3);
-            if(s1 >= SONAR_STOP) {
-               //turnLeft(90);
-            } else if(s3 >= SONAR_STOP) {
-               turnRight(90);
-            } else {
-               pc.printf("IM STUCK HALP\n\t");
-            }
-        }  
-          
-    }
- }
-
- 
\ No newline at end of file