File content as of revision 8:eefbd3880d28:

#include "mbed.h"

//motor select pins
DigitalOut motor_lf(PTE2);
DigitalOut motor_lb(PTE3);
DigitalOut motor_rf(PTE4);
DigitalOut motor_rb(PTE5);

//Frequency of Pulse Width Modulated signal in Hz
#define PWM_FREQ          1000

//PWM pin (Enable 1 and 2)
PwmOut motor_l (PTC2);
PwmOut motor_r (PTE29);

//LED to test
DigitalOut led(LED_RED);

//Hall Effect Sensors
InterruptIn hall_l (PTA1); //Hall Effect Sensor Output RobotLeft
InterruptIn hall_r (PTA2); //Hall Effect Sensor Output RobotRight
float time1_l, time2_l, time3_l, time1_r, time2_r, time3_r;
//Bluetooth Module to debug
Serial blue(PTC4,PTC3);

void set_direction( int direction, float speed, float angle)
{
    /* *******************Example Use*****************
                 set_direction("11", 0.5, 0);  //This would set the robot to go forward, at half speed, straight

                 set_directin( "11", 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)

                 set_direction("00", 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)

                 set_direction("11", 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)

                 set_direction("11", 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)

                 set_direction("11", 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)

                 set_direction("11", -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)

                 set_direction("10", 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%

                 set_direction("10", 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward

    */

    float r_result = (speed + angle); //where angle can be postive and negative and will TURN the robot
    float l_result = (speed - angle); //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
    switch( direction ) {
        case 0x11: { //forward
            motor_r.write(  r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
            //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
            motor_l.write(  l_result < 0 ? 0     :     l_result > 1 ? 1    :       l_result);

            motor_rf=1;
            motor_rb=0;
            motor_lf=1;
            motor_lb=0;
            blue.printf("I'm going forwards, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
        }
        case 0x00: { //backward
            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);

            motor_rf=0;
            motor_rb=1;
            motor_lf=0;
            motor_lb=1;
            blue.printf("I'm going backwards, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);

        }
        case 0x01: {  //spin left  --     Right forward, left backward
            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);

            motor_rf=1;
            motor_rb=0;
            motor_lf=0;
            motor_lb=1;
            blue.printf("I'm spinning left, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);

        }
        case 0x10: {    //spin right
            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);

            motor_rf=0;
            motor_rb=1;
            motor_lf=1;
            motor_lb=0;
            blue.printf("I'm spinning right, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);

        }
    }
}
timer h_r, h_l;
float hall_rt1, hall_lt1, hall_rt2, hall_lt2//hall right time & left time
int h_f_r, h_f_l //hallflagright and left
void hallr() //this just reading the various times.
{
    if (h_f_r==2) {
        start.h_r();
    } else if(h_f_r == 1) {
        hall_rt1 = read.h_r();
        h_f_r = 0;
    } else if (h_f_l == 0) {
        hall_rt2 = read.h_r();
        h_f_r = 1;
    }

}
void halll()
{


}
int main()
{
    h_f_r = 2;
    h_f_l = 2;
    //attaching hall_r and hall_r so that it calls hall() on a rising and falling edge
    hall_r.rise(&hallr());
    hall_r.fall(&hallr());

    hall_l.rise(&halll());
    hall_l.fall(&halll());
//Set PWM frequency to 1000Hz


    motor_l.period( 1.0f / (float) PWM_FREQ);
    motor_r.period( 1.0f / (float) PWM_FREQ);
//Initialise direction to nothing.
    motor_rf=0;
    motor_rb=0;
    motor_lf=0;
    motor_lb=0;
    int direction = 0x11;
    char rem_dir[2]; //remote_direction






    /*BLUETOOTH TEST

    while(1) {


        wait(1);
        blue.printf("Gees a Direction\nFF for forward both, BB for backward both, FB for forward left and back right .. etc. \n");

        while ( blue.readable()==0 ) {}               // loops until there is an input from the user

        for(int i=0; i<2; i++) {
            rem_dir[i] = blue.getc();    //Store incoming response into array
        }

        //0x46 is F, 0x66 is f
        //0x42 is B , 0x62 is b
        if(   (rem_dir[0] == 'F'|'f' )     &&     ( rem_dir[1] == 'F'|'f')   ) {
            direction = 0x11;
            blue.printf("You wanted to go straight\n");
        } else if (   (rem_dir[0] == 'B'|'b' )     &&     ( rem_dir[1] == 'B'|'b')  ) {

            direction = 0x00;
            blue.printf("You wanted to go backwards\n");

        } else if (   (rem_dir[0] == 'B'|'b' )     &&     ( rem_dir[1] == 'F'|'f')   ) {

            direction = 0x01;
            blue.printf("You wanted to spin anti-clockwise\n");

        } else if (   (rem_dir[0] == 'F'|'f' )     &&     ( rem_dir[1] == 'B'|'b') )  {

            direction = 0x10;
            blue.printf("You wanted to spin clockwise\n");
        } else {}
         set_direction(direction, 0.5, 0);

        /* SETTING SPEED AND ANGLE TOO
        blue.printf("Whit speed you going at?\n Send it as a duty percentage");
        while ( blue.readable()==0 ) {}               // loops until there is an input from the user

        speed = input;

        blue.printf("What angle you going at? percentage again (negative percentage for opposite dir)");
                while ( blue.readable()==0 ) {}               // loops until there is an input from the user

        angle = input;
        */

    //set_direction(direction, speed, angle);

    //}

    /* //Also Working Demo
    wait(2);
    led = !led;
    set_direction(0x11, 0.5, 0);  //This would set the robot to go forward, at half speed, straight

    wait(2);
    led = !led;
    set_direction(0x11, 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)

    wait(2);
    led = !led;
    set_direction(0x00, 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)

    wait(2);
    led = !led;
    set_direction(0x11, 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)

    wait(2);
    led = !led;
    set_direction(0x11, 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)

    wait(2);
    led = !led;
    set_direction(0x11, 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)

    wait(2);
    led = !led;
    set_direction(0x11, -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)

    wait(2);
    led = !led;
    set_direction(0x10, 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%

    wait(2);
    led = !led;
    set_direction(0x10, 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward

    }*/


    /*  // working demo

      while(1) { //infinite loop
          //***********************************************
          myled = !myled;

          //Set duty cycle at 100% AKA Full Speed
          motor_r.write(1.0);
          motor_l.write(1.0);

          motor_rf=1;
          motor_rb=0;
          motor_lf=1;
          motor_lb=0; //go forwards
          wait(2);
          //**************************************
          myled = !myled;

          motor_r.write(0.75);
          motor_l.write(0.75);

          motor_rf=1;
          motor_rb=0;
          motor_lf=1;
          motor_lb=0; //go forwards
          //
          wait(2);

          myled = !myled;

          motor_r.write(0.5);
          motor_l.write(0.5);

          motor_rf=1;
          motor_rb=0;
          motor_lf=1;
          motor_lb=0;


          wait(2);
          myled = !myled;

          motor_r.write(0.30);
          motor_l.write(0.30);

          motor_rf=1;
          motor_rb=0;
          motor_lf=1;
          motor_lb=0;

          wait(2);

          myled = !myled;
          motor_r.write(1.0);
          motor_l.write(1.0);

          motor_rf=0;
          motor_rb=1;
          motor_lf=0;
          motor_lb=1; //go backwards
          //
          wait(2);



          myled = !myled;
          motor_r.write(0.5);
          motor_l.write(0.5);

          motor_rf=0;
          motor_rb=0;
          motor_lf=0;
          motor_lb=1; //

          wait(2);
          myled = !myled;

          motor_rf=0;
          motor_rb=1;
          motor_lf=0;
          motor_lb=0; //reverse turn

          wait(2);

          myled = !myled;
          motor_r.write(0.5);
          motor_l.write(1.0);

          motor_rf=1;
          motor_rb=0;
          motor_lf=1;
          motor_lb=0; //go forwards
          //
          wait(4);
      }*/



}