Mechatronics Robotics / Mbed 2 deprecated robotV5

to compare with V6, do not change.

Dependencies:   mbed

mainV5.cpp@0:01c109e18d49, 2017-05-26 (annotated)

Committer:
obrie829
Date:
Fri May 26 12:33:39 2017 +0000
Revision:
0:01c109e18d49
RobotV5 in working order

Who changed what in which revision?

UserRevisionLine numberNew contents of line
obrie829 0:01c109e18d49 1 #include "mbed.h"
obrie829 0:01c109e18d49 2 #include "Brobot.h"
obrie829 0:01c109e18d49 3 #include "IRSensor.h"
obrie829 0:01c109e18d49 4 #include "PID_Control.h"
obrie829 0:01c109e18d49 5
obrie829 0:01c109e18d49 6 DigitalOut led(LED1);
obrie829 0:01c109e18d49 7
obrie829 0:01c109e18d49 8 //Perophery for distance sensors
obrie829 0:01c109e18d49 9 AnalogIn distance(PB_1);
obrie829 0:01c109e18d49 10 DigitalIn button(USER_BUTTON);
obrie829 0:01c109e18d49 11 DigitalOut enable(PC_1);
obrie829 0:01c109e18d49 12 DigitalOut bit0(PH_1);
obrie829 0:01c109e18d49 13 DigitalOut bit1(PC_2);
obrie829 0:01c109e18d49 14 DigitalOut bit2(PC_3);
obrie829 0:01c109e18d49 15 IRSensor sensors[6];
obrie829 0:01c109e18d49 16
obrie829 0:01c109e18d49 17 //indicator leds arround robot
obrie829 0:01c109e18d49 18 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
obrie829 0:01c109e18d49 19
obrie829 0:01c109e18d49 20 //defining the sensors for
obrie829 0:01c109e18d49 21 IRSensor sensor_front( &distance, &bit0, &bit1, &bit2, 0);
obrie829 0:01c109e18d49 22 IRSensor sensor_left( &distance, &bit0, &bit1, &bit2, 5);
obrie829 0:01c109e18d49 23 IRSensor sensor_right( &distance, &bit0, &bit1, &bit2, 1);
obrie829 0:01c109e18d49 24
obrie829 0:01c109e18d49 25 //timer object for ledshow and distance sensor
obrie829 0:01c109e18d49 26 Ticker t1;
obrie829 0:01c109e18d49 27
obrie829 0:01c109e18d49 28 //motor stuff
obrie829 0:01c109e18d49 29 DigitalOut enableMotorDriver(PB_2);
obrie829 0:01c109e18d49 30 PwmOut pwmL(PA_8);
obrie829 0:01c109e18d49 31 PwmOut pwmR(PA_9);
obrie829 0:01c109e18d49 32 int number=0;
obrie829 0:01c109e18d49 33 PID_Control pid;
obrie829 0:01c109e18d49 34
obrie829 0:01c109e18d49 35 Brobot stingray(&pwmL, &pwmR, number);
obrie829 0:01c109e18d49 36
obrie829 0:01c109e18d49 37 //lights up the led according to the sensor signal
obrie829 0:01c109e18d49 38 void ledDistance()
obrie829 0:01c109e18d49 39 {
obrie829 0:01c109e18d49 40 for( int ii = 0; ii<6; ++ii)
obrie829 0:01c109e18d49 41 sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0; // an if statement in one line
obrie829 0:01c109e18d49 42 }
obrie829 0:01c109e18d49 43
obrie829 0:01c109e18d49 44 //blinks at startup and starts ledDistance task
obrie829 0:01c109e18d49 45 void ledShow()
obrie829 0:01c109e18d49 46 {
obrie829 0:01c109e18d49 47 static int timer = 0; // static is only the first time/loop
obrie829 0:01c109e18d49 48 for( int ii = 0; ii<6; ++ii)
obrie829 0:01c109e18d49 49 leds[ii] = !leds[ii];
obrie829 0:01c109e18d49 50
obrie829 0:01c109e18d49 51 //quit ticker and start led distance show
obrie829 0:01c109e18d49 52 if( ++timer > 10) {
obrie829 0:01c109e18d49 53 t1.detach();
obrie829 0:01c109e18d49 54 t1.attach( &ledDistance, 0.01f );
obrie829 0:01c109e18d49 55 }
obrie829 0:01c109e18d49 56 }
obrie829 0:01c109e18d49 57
obrie829 0:01c109e18d49 58 //initiate PWM with period and enable
obrie829 0:01c109e18d49 59 void initPWM()
obrie829 0:01c109e18d49 60 {
obrie829 0:01c109e18d49 61 pwmL.period(0.00005f);
obrie829 0:01c109e18d49 62 pwmR.period(0.00005f);
obrie829 0:01c109e18d49 63 pwmL = 0.5f;
obrie829 0:01c109e18d49 64 pwmR = 0.5f;
obrie829 0:01c109e18d49 65 enableMotorDriver =1;
obrie829 0:01c109e18d49 66 }
obrie829 0:01c109e18d49 67
obrie829 0:01c109e18d49 68 // Collision avoidance
obrie829 0:01c109e18d49 69 void move()
obrie829 0:01c109e18d49 70 {
obrie829 0:01c109e18d49 71 wait(0.01);
obrie829 0:01c109e18d49 72 float speed = 0.25;
obrie829 0:01c109e18d49 73
obrie829 0:01c109e18d49 74 float e = sensor_left.read() - sensor_right.read();
obrie829 0:01c109e18d49 75 float diff = pid.calc( 0.0f+e, 0.005f );
obrie829 0:01c109e18d49 76
obrie829 0:01c109e18d49 77 pwmL = 0.5f - diff + speed;
obrie829 0:01c109e18d49 78 pwmR = 0.5f - diff - speed;
obrie829 0:01c109e18d49 79
obrie829 0:01c109e18d49 80 if(sensor_front.read() <=0.3f) { // when approaching normal to wall
obrie829 0:01c109e18d49 81 stingray.stop();
obrie829 0:01c109e18d49 82 int direction=rand()%2 ; // so there is variablility in the robots path
obrie829 0:01c109e18d49 83 if(direction==0) {
obrie829 0:01c109e18d49 84 stingray.rotate(0.1f); // and value between 0 and 0.25
obrie829 0:01c109e18d49 85 wait(0.5);
obrie829 0:01c109e18d49 86 } else if (direction==1) {
obrie829 0:01c109e18d49 87 stingray.rotate(-0.1f);
obrie829 0:01c109e18d49 88 wait(0.5);
obrie829 0:01c109e18d49 89 }
obrie829 0:01c109e18d49 90 }
obrie829 0:01c109e18d49 91 }
obrie829 0:01c109e18d49 92
obrie829 0:01c109e18d49 93 int main()
obrie829 0:01c109e18d49 94 {
obrie829 0:01c109e18d49 95 enableMotorDriver = 0; //safety precaution
obrie829 0:01c109e18d49 96 enable = 1 ; //for debugging and output
obrie829 0:01c109e18d49 97
obrie829 0:01c109e18d49 98 t1.attach( &ledShow, 0.05f ); //ticker
obrie829 0:01c109e18d49 99
obrie829 0:01c109e18d49 100 enum robot_states {idle=0, search}; //so that we can reference states by name vs number
obrie829 0:01c109e18d49 101 int robot_state=idle; // define and start in idle state
obrie829 0:01c109e18d49 102
obrie829 0:01c109e18d49 103 //initialize distance sensors
obrie829 0:01c109e18d49 104 for( int ii = 0; ii<6; ++ii)
obrie829 0:01c109e18d49 105 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
obrie829 0:01c109e18d49 106
obrie829 0:01c109e18d49 107 // kp ki kd min max
obrie829 0:01c109e18d49 108 pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000);
obrie829 0:01c109e18d49 109
obrie829 0:01c109e18d49 110 while(1) {
obrie829 0:01c109e18d49 111 wait( 0.1f ); //rquired to allow time for ticker
obrie829 0:01c109e18d49 112
obrie829 0:01c109e18d49 113 switch(robot_state) {
obrie829 0:01c109e18d49 114 case idle:
obrie829 0:01c109e18d49 115 if(button == 0) { // 0 is pressed
obrie829 0:01c109e18d49 116 initPWM(); // sets period
obrie829 0:01c109e18d49 117 robot_state=search; // next state
obrie829 0:01c109e18d49 118 }
obrie829 0:01c109e18d49 119 break;
obrie829 0:01c109e18d49 120
obrie829 0:01c109e18d49 121 case search:
obrie829 0:01c109e18d49 122 move();
obrie829 0:01c109e18d49 123 break;
obrie829 0:01c109e18d49 124 }
obrie829 0:01c109e18d49 125 }
obrie829 0:01c109e18d49 126 }