der Roboter / S1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   SensoryTest

Fork of btbee by Nikolas Goldin

main.cpp@16:2b437630e29e, 2014-05-21 (annotated)

Committer:
mmpeter
Date:
Wed May 21 12:59:30 2014 +0000
Revision:
16:2b437630e29e
Parent:
15:cc5b8a858c6e
Child:
17:a6cb4c42308c
testing turnaround

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mmpeter 8:fd8a30a6923d 1 #include "mbed.h"
mmpeter 8:fd8a30a6923d 2 #include "m3pi_ng.h"
mmpeter 8:fd8a30a6923d 3
mmpeter 16:2b437630e29e 4 void turn_around(int array[5],float speed);
mmpeter 16:2b437630e29e 5
mmpeter 8:fd8a30a6923d 6 m3pi thinggy;
mmpeter 8:fd8a30a6923d 7
mmpeter 8:fd8a30a6923d 8
mmpeter 8:fd8a30a6923d 9 int main() {
mmpeter 8:fd8a30a6923d 10
mmpeter 16:2b437630e29e 11 float speed = 0.25;
mmpeter 11:45172e28ecb2 12 float correction;
mmpeter 16:2b437630e29e 13 float k = -0.3;
mmpeter 16:2b437630e29e 14 int sensor[5];
mmpeter 9:6ae4359b73df 15
mmpeter 9:6ae4359b73df 16 thinggy.locate(0,1);
mmpeter 15:cc5b8a858c6e 17 thinggy.printf("HellzYa");
mmpeter 8:fd8a30a6923d 18
mmpeter 10:7ef481d28dcd 19 wait(1.0);
mmpeter 8:fd8a30a6923d 20
mmpeter 8:fd8a30a6923d 21 thinggy.sensor_auto_calibrate();
mmpeter 8:fd8a30a6923d 22 while(1) {
mmpeter 11:45172e28ecb2 23
mmpeter 16:2b437630e29e 24 thinggy.raw_sensor(sensor);
mmpeter 16:2b437630e29e 25
mmpeter 16:2b437630e29e 26
mmpeter 16:2b437630e29e 27 turn_around(sensor, speed);
mmpeter 8:fd8a30a6923d 28 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 11:45172e28ecb2 29 float position = thinggy.line_position();
mmpeter 11:45172e28ecb2 30 correction = k*(position);
mmpeter 11:45172e28ecb2 31
mmpeter 11:45172e28ecb2 32 //speed limiting for right motor
mmpeter 11:45172e28ecb2 33 if(speed + correction > 1){
mmpeter 15:cc5b8a858c6e 34 thinggy.right_motor(0.6);
mmpeter 15:cc5b8a858c6e 35 }
mmpeter 16:2b437630e29e 36
mmpeter 11:45172e28ecb2 37 else{
mmpeter 11:45172e28ecb2 38 thinggy.right_motor(speed+correction);
mmpeter 11:45172e28ecb2 39 }
mmpeter 11:45172e28ecb2 40
mmpeter 11:45172e28ecb2 41 //speed limiting for left motor
mmpeter 11:45172e28ecb2 42 if(speed - correction > 1){
mmpeter 15:cc5b8a858c6e 43 thinggy.left_motor(0.6);
mmpeter 15:cc5b8a858c6e 44 }
mmpeter 16:2b437630e29e 45
mmpeter 16:2b437630e29e 46
mmpeter 11:45172e28ecb2 47 else{
mmpeter 10:7ef481d28dcd 48 thinggy.left_motor(speed-correction);
mmpeter 11:45172e28ecb2 49 }
mmpeter 16:2b437630e29e 50
mmpeter 16:2b437630e29e 51 }
mmpeter 16:2b437630e29e 52 }
mmpeter 16:2b437630e29e 53
mmpeter 16:2b437630e29e 54 ////-------------------------------------------------------------------------------------////
mmpeter 16:2b437630e29e 55 ////------------------------------TURN AROUND FUNCTION-----------------------------------////
mmpeter 16:2b437630e29e 56 ////-------------------------------------------------------------------------------------////
mmpeter 13:4016c362b0d5 57
mmpeter 16:2b437630e29e 58 void turn_around(int array[5],float speed){
mmpeter 16:2b437630e29e 59 // Declaring Variables
mmpeter 16:2b437630e29e 60 int threshold = 200;
mmpeter 13:4016c362b0d5 61
mmpeter 16:2b437630e29e 62 bool turn = false;
mmpeter 16:2b437630e29e 63 //NOT NEEDED : int amt = 0;
mmpeter 16:2b437630e29e 64 // Check to see if all sensors are lower than threshold
mmpeter 16:2b437630e29e 65
mmpeter 16:2b437630e29e 66 while(!turn){
mmpeter 16:2b437630e29e 67
mmpeter 16:2b437630e29e 68 int x = 0;
mmpeter 16:2b437630e29e 69
mmpeter 16:2b437630e29e 70 for(int i = 1; i < 4; i++){
mmpeter 16:2b437630e29e 71 x = x + array[i];
mmpeter 16:2b437630e29e 72 }
mmpeter 16:2b437630e29e 73
mmpeter 16:2b437630e29e 74 int x_avg = x/3;
mmpeter 16:2b437630e29e 75
mmpeter 16:2b437630e29e 76 if(x_avg < threshold){
mmpeter 16:2b437630e29e 77 turn = true;
mmpeter 16:2b437630e29e 78 thinggy.stop();
mmpeter 16:2b437630e29e 79 }
mmpeter 16:2b437630e29e 80 }
mmpeter 16:2b437630e29e 81
mmpeter 16:2b437630e29e 82 while(turn){
mmpeter 16:2b437630e29e 83
mmpeter 16:2b437630e29e 84 int y = 0;
mmpeter 16:2b437630e29e 85 thinggy.left_motor(speed);
mmpeter 16:2b437630e29e 86 thinggy.right_motor(-speed);
mmpeter 16:2b437630e29e 87
mmpeter 16:2b437630e29e 88 for(int i = 1; i < 4; i++){
mmpeter 16:2b437630e29e 89 y = y + array[i];
mmpeter 16:2b437630e29e 90 }
mmpeter 16:2b437630e29e 91
mmpeter 16:2b437630e29e 92 int y_avg = y/3;
mmpeter 16:2b437630e29e 93
mmpeter 16:2b437630e29e 94 if(y_avg > threshold){
mmpeter 16:2b437630e29e 95 turn = false;
mmpeter 16:2b437630e29e 96 }
mmpeter 16:2b437630e29e 97 break;
mmpeter 16:2b437630e29e 98 }
mmpeter 16:2b437630e29e 99
mmpeter 16:2b437630e29e 100 }