JJ Dekker / Mbed 2 deprecated Totaleprogramma1

The project is not done yet

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

Fork of Totaleprogramma by jordy morsinkhof

main.cpp@1:da390b3b1330, 2015-03-02 (annotated)

Committer:
juliandekker
Date:
Mon Mar 02 10:58:30 2015 +0000
Revision:
1:da390b3b1330
Parent:
0:345f76c72b9a 
xbox controller robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
347467 0:345f76c72b9a 1 #define DEBUG 0
347467 0:345f76c72b9a 2 #include "mbed.h"
347467 0:345f76c72b9a 3 #include "rtos.h"
347467 0:345f76c72b9a 4 #include "USBHostXpad.h"
347467 0:345f76c72b9a 5 #include "utils.h"
347467 0:345f76c72b9a 6 #include <cmath>
347467 0:345f76c72b9a 7
juliandekker 1:da390b3b1330 8 Serial pc (USBTX, USBRX);
juliandekker 1:da390b3b1330 9 I2C i2cMod (p28, p27);
juliandekker 1:da390b3b1330 10 DigitalOut handmode(p5); // hand moet 1 worden (niet connected
juliandekker 1:da390b3b1330 11 DigitalIn automode(p6); // auto
juliandekker 1:da390b3b1330 12 DigitalIn externemode(p7); // extern
juliandekker 1:da390b3b1330 13 DigitalIn aanuit(p8); // aan uit
juliandekker 1:da390b3b1330 14 DigitalOut motorLeftFrontDirAV(p9); //motor lv 1= vooruit
juliandekker 1:da390b3b1330 15 DigitalOut motorRightFrontDirAV(p10); //MOTOR rv 1= vooruit
juliandekker 1:da390b3b1330 16 DigitalOut motorLeftRearDirAV(p11); //motor la 1= vooruit
juliandekker 1:da390b3b1330 17 DigitalOut motorRightRearDirAV(p12); //MOTOR ra 1= vooruit
juliandekker 1:da390b3b1330 18 PwmOut motorLeftFrontVelAV(p21); //motor lv
juliandekker 1:da390b3b1330 19 PwmOut motorRightFrontVelAV (p22); //Motor rv
juliandekker 1:da390b3b1330 20 PwmOut motorLeftRearVelAV(p23); //motor la
juliandekker 1:da390b3b1330 21 PwmOut motorRightRearVelAV (p24); //Motor ra
juliandekker 1:da390b3b1330 22 AnalogIn pot(p19); //potmeter
juliandekker 1:da390b3b1330 23 AnalogIn laserLeft(p15); //lasersensorwaarde lv2
juliandekker 1:da390b3b1330 24 AnalogIn laserRight(p16); //lasersensorwaarde rv2
juliandekker 1:da390b3b1330 25 DigitalOut led1(LED1);
juliandekker 1:da390b3b1330 26 DigitalOut led2(LED2);
juliandekker 1:da390b3b1330 27 DigitalOut led3(LED3);
juliandekker 1:da390b3b1330 28 DigitalOut led4(LED4);
juliandekker 1:da390b3b1330 29
347467 0:345f76c72b9a 30
juliandekker 1:da390b3b1330 31 float SonarLeft; // afstandswaarde ultrasoon sensor 1(links?)
juliandekker 1:da390b3b1330 32 float SonarRight; // afstandswaarde ultrasoon sensor 2 (rechts?)
juliandekker 1:da390b3b1330 33 float AS = 0.5; // actuele snelheid
juliandekker 1:da390b3b1330 34 float z = 0.5; // percentage voor het draaien van de motoren
juliandekker 1:da390b3b1330 35 float j = 0.75; // persentage voor het draaien van de motoren
juliandekker 1:da390b3b1330 36 float Q = 300; // De afstand vanaf waar de ultrasonesensor iets moet detecteren in CM. // groter dan een waarde 300 = 3 meter detecteerd de sensor iets
juliandekker 1:da390b3b1330 37 float I = 1.5; // de afstand vanaf waar de infraroodsensor iets moet detecteren.
juliandekker 1:da390b3b1330 38 float x = 0.5; // persentage voor het draaien van de motoren
juliandekker 1:da390b3b1330 39 float y = 0.25; // persentage voor het draaien van de motoren
juliandekker 1:da390b3b1330 40 float motorAcc = 0.3; // Motor versnelling pwm per seconde
juliandekker 1:da390b3b1330 41 float motorDec = 0.5; // Motor vertraging pwm per seconde
juliandekker 1:da390b3b1330 42 bool motorLeftFrontDirSP = 0; //motordirection leftfront setpoint
juliandekker 1:da390b3b1330 43 bool motorRightFrontDirSP = 0; //motordirection leftfront setpoint
juliandekker 1:da390b3b1330 44 bool motorLeftRearDirSP = 0; //motordirection leftfront setpoint
juliandekker 1:da390b3b1330 45 bool motorRightRearDirSP = 0; //motordirection leftfront setpoint
juliandekker 1:da390b3b1330 46 float motorLeftFrontVelSP =0; // motor snelheid leftfront setpoint
juliandekker 1:da390b3b1330 47 float motorRightFrontVelSP =0; // motor snelheid leftfront setpoint
juliandekker 1:da390b3b1330 48 float motorLeftRearVelSP =0; // motor snelheid leftfront setpoint
juliandekker 1:da390b3b1330 49 float motorRightRearVelSP =0; // motor snelheid leftfront setpoint
347467 0:345f76c72b9a 50
347467 0:345f76c72b9a 51 Xbox360ControllerState controlState;
juliandekker 1:da390b3b1330 52
juliandekker 1:da390b3b1330 53 // Turn right program
juliandekker 1:da390b3b1330 54 void turnRight(){
juliandekker 1:da390b3b1330 55 led3 = 1; //rechts
juliandekker 1:da390b3b1330 56 motorLeftFrontDirSP = 1; //lv
juliandekker 1:da390b3b1330 57 motorRightFrontDirSP = 0; //rv
juliandekker 1:da390b3b1330 58 motorLeftRearDirSP = 1; //la
juliandekker 1:da390b3b1330 59 motorRightRearDirSP = 0; //Ra
juliandekker 1:da390b3b1330 60 motorLeftFrontVelSP = AS*j;
juliandekker 1:da390b3b1330 61 motorRightFrontVelSP = AS*z;
juliandekker 1:da390b3b1330 62 motorLeftRearVelSP = AS*j;
juliandekker 1:da390b3b1330 63 motorRightRearVelSP = AS*z;
juliandekker 1:da390b3b1330 64 pc.printf("rechts\r\n");
juliandekker 1:da390b3b1330 65 }
juliandekker 1:da390b3b1330 66
juliandekker 1:da390b3b1330 67 // Turn Left program
juliandekker 1:da390b3b1330 68 void turnLeft(){
juliandekker 1:da390b3b1330 69 motorLeftFrontDirSP = 1; //lv
juliandekker 1:da390b3b1330 70 motorRightFrontDirSP = 1; //rv
juliandekker 1:da390b3b1330 71 motorLeftRearDirSP = 1; //La
juliandekker 1:da390b3b1330 72 motorRightRearDirSP = 1; //Ra
juliandekker 1:da390b3b1330 73 motorLeftFrontVelSP = AS*x;
juliandekker 1:da390b3b1330 74 motorRightFrontVelSP = AS*y;
juliandekker 1:da390b3b1330 75 motorLeftRearVelSP = AS*x;
juliandekker 1:da390b3b1330 76 motorRightRearVelSP = AS*y;
juliandekker 1:da390b3b1330 77 pc.printf("links\r\n");
juliandekker 1:da390b3b1330 78 }
juliandekker 1:da390b3b1330 79
juliandekker 1:da390b3b1330 80 void forward (){
juliandekker 1:da390b3b1330 81 motorLeftFrontDirSP = 1; //lv
juliandekker 1:da390b3b1330 82 motorRightFrontDirSP = 1; //rv
juliandekker 1:da390b3b1330 83 motorLeftRearDirSP = 1; //La
juliandekker 1:da390b3b1330 84 motorRightRearDirSP = 1; //Ra
juliandekker 1:da390b3b1330 85 motorLeftFrontVelSP = AS;
juliandekker 1:da390b3b1330 86 motorRightFrontVelSP = AS;
juliandekker 1:da390b3b1330 87 motorLeftRearVelSP = AS;
juliandekker 1:da390b3b1330 88 motorRightRearVelSP = AS;
juliandekker 1:da390b3b1330 89 pc.printf("rechtsvooruit\r\n");
juliandekker 1:da390b3b1330 90 }
juliandekker 1:da390b3b1330 91
juliandekker 1:da390b3b1330 92 void reverse(){
juliandekker 1:da390b3b1330 93 motorLeftFrontDirSP = 0; //lv
juliandekker 1:da390b3b1330 94 motorRightFrontDirSP = 0; //rv
juliandekker 1:da390b3b1330 95 motorLeftRearDirSP = 0; //La
juliandekker 1:da390b3b1330 96 motorRightRearDirSP = 0; //Ra
juliandekker 1:da390b3b1330 97 motorLeftFrontVelSP = AS;
juliandekker 1:da390b3b1330 98 motorRightFrontVelSP = AS;
juliandekker 1:da390b3b1330 99 motorLeftRearVelSP = AS;
juliandekker 1:da390b3b1330 100 motorRightRearVelSP = AS;
juliandekker 1:da390b3b1330 101 pc.printf("achteruit\r\n");
juliandekker 1:da390b3b1330 102 }
347467 0:345f76c72b9a 103
juliandekker 1:da390b3b1330 104 void halt(){
juliandekker 1:da390b3b1330 105 motorLeftFrontDirSP = 0; //lv
juliandekker 1:da390b3b1330 106 motorRightFrontDirSP = 0; //rv
juliandekker 1:da390b3b1330 107 motorLeftRearDirSP = 0; //La
juliandekker 1:da390b3b1330 108 motorRightRearDirSP = 0; //Ra
juliandekker 1:da390b3b1330 109 motorLeftFrontVelSP = 0;
juliandekker 1:da390b3b1330 110 motorRightFrontVelSP = 0;
juliandekker 1:da390b3b1330 111 motorLeftRearVelSP = 0;
juliandekker 1:da390b3b1330 112 motorRightRearVelSP = 0;
juliandekker 1:da390b3b1330 113 pc.printf("stop\r\n");
juliandekker 1:da390b3b1330 114 }
juliandekker 1:da390b3b1330 115 ///todo nul moet nog stilstaan worden
juliandekker 1:da390b3b1330 116 //Thread MotorController
juliandekker 1:da390b3b1330 117 void thread_motorControl(void const* arg){
juliandekker 1:da390b3b1330 118 int controlInterval = 100;// was100
juliandekker 1:da390b3b1330 119 while(1){
juliandekker 1:da390b3b1330 120 Thread::wait(controlInterval);
juliandekker 1:da390b3b1330 121 //MotorLeftFront
juliandekker 1:da390b3b1330 122 if(motorLeftFrontDirSP == motorLeftFrontDirAV){
juliandekker 1:da390b3b1330 123 if (motorLeftFrontVelSP > motorLeftFrontVelAV){
juliandekker 1:da390b3b1330 124 motorLeftFrontVelAV=motorLeftFrontVelAV+(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 125 }
juliandekker 1:da390b3b1330 126 else if(motorLeftFrontVelSP < motorLeftFrontVelAV){
juliandekker 1:da390b3b1330 127 motorLeftFrontVelAV=motorLeftFrontVelAV-(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 128 }
juliandekker 1:da390b3b1330 129 }
juliandekker 1:da390b3b1330 130 else {
juliandekker 1:da390b3b1330 131 //regel naar nul
juliandekker 1:da390b3b1330 132 if ((motorLeftFrontVelAV-(motorDec/controlInterval*10))> 0){
juliandekker 1:da390b3b1330 133 motorLeftFrontVelAV= motorLeftFrontVelAV-(motorDec/controlInterval*10);
juliandekker 1:da390b3b1330 134 }
juliandekker 1:da390b3b1330 135 else{
juliandekker 1:da390b3b1330 136 motorLeftFrontDirAV = not motorLeftFrontDirAV;
juliandekker 1:da390b3b1330 137 }
juliandekker 1:da390b3b1330 138 }
juliandekker 1:da390b3b1330 139 //MotorRightFront
juliandekker 1:da390b3b1330 140 if(motorRightFrontDirSP == motorRightFrontDirAV){
juliandekker 1:da390b3b1330 141 if (motorRightFrontVelSP > motorRightFrontVelAV){
juliandekker 1:da390b3b1330 142 motorRightFrontVelAV=motorRightFrontVelAV+(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 143 }
juliandekker 1:da390b3b1330 144 else if(motorRightFrontVelSP < motorRightFrontVelAV){
juliandekker 1:da390b3b1330 145 motorRightFrontVelAV=motorRightFrontVelAV-(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 146 }
juliandekker 1:da390b3b1330 147 }
juliandekker 1:da390b3b1330 148 else {
juliandekker 1:da390b3b1330 149 //regel naar nul
juliandekker 1:da390b3b1330 150 if ((motorRightFrontVelAV-(motorDec/controlInterval*10))> 0){
juliandekker 1:da390b3b1330 151 motorRightFrontVelAV= motorRightFrontVelAV-(motorDec/controlInterval*10);
juliandekker 1:da390b3b1330 152 }
juliandekker 1:da390b3b1330 153 else{
juliandekker 1:da390b3b1330 154 motorRightFrontDirAV = not motorRightFrontDirAV;
juliandekker 1:da390b3b1330 155 }
juliandekker 1:da390b3b1330 156 }
juliandekker 1:da390b3b1330 157 //motorLeftRear
juliandekker 1:da390b3b1330 158 if(motorLeftRearDirSP == motorLeftRearDirAV){
juliandekker 1:da390b3b1330 159 if (motorLeftRearVelSP > motorLeftRearVelAV){
juliandekker 1:da390b3b1330 160 motorLeftRearVelAV=motorLeftRearVelAV+(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 161 }
juliandekker 1:da390b3b1330 162 else if(motorLeftRearVelSP < motorLeftRearVelAV){
juliandekker 1:da390b3b1330 163 motorLeftRearVelAV=motorLeftRearVelAV-(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 164 }
juliandekker 1:da390b3b1330 165 }
juliandekker 1:da390b3b1330 166 else {
juliandekker 1:da390b3b1330 167 //regel naar nul
juliandekker 1:da390b3b1330 168 if ((motorLeftRearVelAV-(motorDec/controlInterval*10))> 0){
juliandekker 1:da390b3b1330 169 motorLeftRearVelAV= motorLeftRearVelAV-(motorDec/controlInterval*10);
juliandekker 1:da390b3b1330 170 }
juliandekker 1:da390b3b1330 171 else{
juliandekker 1:da390b3b1330 172 motorLeftRearDirAV = not motorLeftRearDirAV;
juliandekker 1:da390b3b1330 173 }
juliandekker 1:da390b3b1330 174 }
juliandekker 1:da390b3b1330 175 //motorRightRear
juliandekker 1:da390b3b1330 176 if(motorRightRearDirSP == motorRightRearDirAV){
juliandekker 1:da390b3b1330 177 if (motorRightRearVelSP > motorRightRearVelAV){
juliandekker 1:da390b3b1330 178 motorRightRearVelAV=motorRightRearVelAV+(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 179 }
juliandekker 1:da390b3b1330 180 else if(motorRightRearVelSP < motorRightRearVelAV){
juliandekker 1:da390b3b1330 181 motorRightRearVelAV=motorRightRearVelAV-(motorAcc/controlInterval*10);
juliandekker 1:da390b3b1330 182 }
juliandekker 1:da390b3b1330 183 }
juliandekker 1:da390b3b1330 184 else {
juliandekker 1:da390b3b1330 185 //regel naar nul
juliandekker 1:da390b3b1330 186 if ((motorRightRearVelAV-(motorDec/controlInterval*10))> 0){
juliandekker 1:da390b3b1330 187 motorRightRearVelAV= motorRightRearVelAV-(motorDec/controlInterval*10);
juliandekker 1:da390b3b1330 188 }
juliandekker 1:da390b3b1330 189 else{
juliandekker 1:da390b3b1330 190 motorRightRearDirAV = not motorRightRearDirAV;
juliandekker 1:da390b3b1330 191 }
juliandekker 1:da390b3b1330 192 }
juliandekker 1:da390b3b1330 193 pc.printf ("dirAV : %d ", motorLeftFrontDirAV.read());
juliandekker 1:da390b3b1330 194 pc.printf ("velocityAV :%F", motorLeftFrontVelAV.read());
juliandekker 1:da390b3b1330 195 pc.printf ("dirSP : %d ", motorLeftFrontDirSP);
juliandekker 1:da390b3b1330 196 pc.printf ("velocitySP :%F\r\n", motorLeftFrontVelSP);
juliandekker 1:da390b3b1330 197 }
juliandekker 1:da390b3b1330 198 }
juliandekker 1:da390b3b1330 199
juliandekker 1:da390b3b1330 200 // programma tbv uitlezen ultasoon sensoren
347467 0:345f76c72b9a 201 int i2cAddress1 = 0xF2;
347467 0:345f76c72b9a 202 int i2cAddress2 = 0xE0;
347467 0:345f76c72b9a 203
347467 0:345f76c72b9a 204 void sendStartRangingCommand1(void){
347467 0:345f76c72b9a 205 const char command[] = {0x00, 0x51};
347467 0:345f76c72b9a 206 i2cMod.write(i2cAddress1, command, 2);
347467 0:345f76c72b9a 207 }
347467 0:345f76c72b9a 208 void sendStartRangingCommand2(void){
347467 0:345f76c72b9a 209 const char command[] = {0x00, 0x51};
347467 0:345f76c72b9a 210 i2cMod.write(i2cAddress2, command, 2);
347467 0:345f76c72b9a 211 }
347467 0:345f76c72b9a 212 int readRange1(void){
347467 0:345f76c72b9a 213 const char command[] = {0x02}; //Address of range register
347467 0:345f76c72b9a 214 char response[] = {0x00, 0x00};
347467 0:345f76c72b9a 215 i2cMod.write(i2cAddress1, command, 1, 1); //Send command
347467 0:345f76c72b9a 216 i2cMod.read(i2cAddress1, response, 2); //Read 16bits result
347467 0:345f76c72b9a 217 int range = (response[0]<<8)+response[1]; //Shift two bytes into int
347467 0:345f76c72b9a 218 return range;
347467 0:345f76c72b9a 219 }
347467 0:345f76c72b9a 220
347467 0:345f76c72b9a 221 int readRange2(void){
347467 0:345f76c72b9a 222 const char command[] = {0x02}; //Address of range register
347467 0:345f76c72b9a 223 char response[] = {0x00, 0x00};
347467 0:345f76c72b9a 224 i2cMod.write(i2cAddress2, command, 1, 1); //Send command
347467 0:345f76c72b9a 225 i2cMod.read(i2cAddress2, response, 2); //Read 16bits result
347467 0:345f76c72b9a 226 int range = (response[0]<<8)+response[1]; //Shift two bytes into int
347467 0:345f76c72b9a 227 return range;
347467 0:345f76c72b9a 228 }
347467 0:345f76c72b9a 229
juliandekker 1:da390b3b1330 230 // Thread Sonar ranging
juliandekker 1:da390b3b1330 231 void thread_sonarControl(void const* arg){
juliandekker 1:da390b3b1330 232 while (1){
juliandekker 1:da390b3b1330 233 // sensoren automatisch
juliandekker 1:da390b3b1330 234 sendStartRangingCommand1();
juliandekker 1:da390b3b1330 235 Thread::wait(0.07);
juliandekker 1:da390b3b1330 236 SonarLeft = readRange1();
juliandekker 1:da390b3b1330 237 Thread::wait (0.2);
juliandekker 1:da390b3b1330 238 sendStartRangingCommand2();
juliandekker 1:da390b3b1330 239 Thread::wait(0.07);
juliandekker 1:da390b3b1330 240 SonarRight = readRange2();
juliandekker 1:da390b3b1330 241 }
347467 0:345f76c72b9a 242 }
347467 0:345f76c72b9a 243
juliandekker 1:da390b3b1330 244 // Programm tbv xbox controller
juliandekker 1:da390b3b1330 245 void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){
juliandekker 1:da390b3b1330 246 controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
juliandekker 1:da390b3b1330 247 }
347467 0:345f76c72b9a 248
juliandekker 1:da390b3b1330 249 // programma tbv xbox controller
347467 0:345f76c72b9a 250 void thread_controller(void const* arg){
347467 0:345f76c72b9a 251 USBHostXpad controller;
347467 0:345f76c72b9a 252 controller.attachEvent(&onControlInput);
347467 0:345f76c72b9a 253
juliandekker 1:da390b3b1330 254 while(1){
juliandekker 1:da390b3b1330 255 // connect programma
347467 0:345f76c72b9a 256 bool wasdisconnected = true;
347467 0:345f76c72b9a 257 //acts as a failsafe
juliandekker 1:da390b3b1330 258 while(controller.connected()){
juliandekker 1:da390b3b1330 259 if(wasdisconnected){
347467 0:345f76c72b9a 260 controller.led( USBHostXpad::LED1_ON );
juliandekker 1:da390b3b1330 261 //stop, wait for controller to reconnect for a second.
347467 0:345f76c72b9a 262 wasdisconnected = false;
347467 0:345f76c72b9a 263 }
juliandekker 1:da390b3b1330 264 if( controlState.buttons & USBHostXpad::XPAD_PAD_X){
347467 0:345f76c72b9a 265 //Stoppen
juliandekker 1:da390b3b1330 266 halt();
juliandekker 1:da390b3b1330 267 }
juliandekker 1:da390b3b1330 268 //dpad xbox controller
juliandekker 1:da390b3b1330 269 if(controlState.buttons & (USBHostXpad::XPAD_HAT_UP | USBHostXpad::XPAD_HAT_DOWN | USBHostXpad::XPAD_HAT_LEFT | USBHostXpad::XPAD_HAT_RIGHT | USBHostXpad::XPAD_PAD_Y)& automode == 0 & externemode == 0 & aanuit == 1){
juliandekker 1:da390b3b1330 270 //vooruit
juliandekker 1:da390b3b1330 271 pc.printf (" handmatig \n ");
juliandekker 1:da390b3b1330 272 if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){
juliandekker 1:da390b3b1330 273 forward();
juliandekker 1:da390b3b1330 274 }
juliandekker 1:da390b3b1330 275 //Achteruit
juliandekker 1:da390b3b1330 276 else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){
juliandekker 1:da390b3b1330 277 reverse();
juliandekker 1:da390b3b1330 278 }
juliandekker 1:da390b3b1330 279 //links
juliandekker 1:da390b3b1330 280 else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){
juliandekker 1:da390b3b1330 281 turnLeft();
juliandekker 1:da390b3b1330 282 }
juliandekker 1:da390b3b1330 283 //rechts
juliandekker 1:da390b3b1330 284 else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){
juliandekker 1:da390b3b1330 285 turnRight();
juliandekker 1:da390b3b1330 286 }
347467 0:345f76c72b9a 287 }
347467 0:345f76c72b9a 288 }
juliandekker 1:da390b3b1330 289 //stop, wait for controller to reconnect for a second.
juliandekker 1:da390b3b1330 290 Thread::wait(500); /// was 500
347467 0:345f76c72b9a 291 controller.connect();
347467 0:345f76c72b9a 292 }
347467 0:345f76c72b9a 293 }
347467 0:345f76c72b9a 294
juliandekker 1:da390b3b1330 295 //////////////////////////////////////////////////////////////////
juliandekker 1:da390b3b1330 296 int main() {
juliandekker 1:da390b3b1330 297 Thread t_controller(thread_controller);
juliandekker 1:da390b3b1330 298 Thread t_motorControl(thread_motorControl);
juliandekker 1:da390b3b1330 299 Thread t_sonarControl(thread_sonarControl);
347467 0:345f76c72b9a 300 while(1) {
juliandekker 1:da390b3b1330 301 if (aanuit == 1 & automode == 0 & externemode == 0){
juliandekker 1:da390b3b1330 302 handmode = 1;
juliandekker 1:da390b3b1330 303 pc.printf (" handmode \r\n ");
juliandekker 1:da390b3b1330 304
juliandekker 1:da390b3b1330 305 }
juliandekker 1:da390b3b1330 306 if(aanuit == 1 & automode ==1){
juliandekker 1:da390b3b1330 307 //AS =pot*3.3; aanzetten weer
juliandekker 1:da390b3b1330 308 //alles iets detecteerd dus draaien
juliandekker 1:da390b3b1330 309 if (SonarLeft<=Q && SonarRight<=Q && laserLeft>=I && laserRight>=I){
juliandekker 1:da390b3b1330 310 turnRight();
juliandekker 1:da390b3b1330 311 }
juliandekker 1:da390b3b1330 312 //rv1 rv2 en lv1 iets gedetecteerd hebben, naar links draaien
juliandekker 1:da390b3b1330 313 else if (SonarLeft<=Q && SonarRight<=Q && laserLeft<=I && laserRight>=I){
juliandekker 1:da390b3b1330 314 turnLeft();
juliandekker 1:da390b3b1330 315 }
juliandekker 1:da390b3b1330 316 //lv1 lv2 en rv1 iets gedetecteerd hebben, naar rechts draaien)
juliandekker 1:da390b3b1330 317 else if (SonarLeft<=Q && SonarRight<=Q && laserLeft>=I && laserRight<=I){
juliandekker 1:da390b3b1330 318 turnRight();
juliandekker 1:da390b3b1330 319 }
juliandekker 1:da390b3b1330 320 //rv1 rv2 iets gedetecteerd hebben, naar links draaien)
juliandekker 1:da390b3b1330 321 else if (SonarLeft>Q && SonarRight<Q && laserLeft<I && laserRight>I){
juliandekker 1:da390b3b1330 322 turnLeft();
juliandekker 1:da390b3b1330 323 }
juliandekker 1:da390b3b1330 324 //lv1 lv2 iets gedetecteerd hebben, naar rechts draaien)
juliandekker 1:da390b3b1330 325 else if (SonarLeft<Q && SonarRight>Q && laserLeft>I && laserRight<I){
juliandekker 1:da390b3b1330 326 turnRight();
juliandekker 1:da390b3b1330 327 }
juliandekker 1:da390b3b1330 328 //lv1 rv1 iets gedetecteerd hebben, naar links of rechts draaien)
juliandekker 1:da390b3b1330 329 else if (SonarLeft<Q && SonarRight>Q && laserLeft<I && laserRight<I){
juliandekker 1:da390b3b1330 330 if ( laserLeft>=laserRight){
juliandekker 1:da390b3b1330 331 turnLeft();
juliandekker 1:da390b3b1330 332 }
juliandekker 1:da390b3b1330 333 else if ( laserLeft<=laserRight ){
juliandekker 1:da390b3b1330 334 turnRight();
347467 0:345f76c72b9a 335 }
347467 0:345f76c72b9a 336 }
juliandekker 1:da390b3b1330 337 //lv1 iets gedetecteerd heeft, naar rechts draaien)
juliandekker 1:da390b3b1330 338 else if (SonarLeft<Q && SonarRight>Q && laserLeft<I && laserRight<I){
juliandekker 1:da390b3b1330 339 turnRight();
347467 0:345f76c72b9a 340 }
juliandekker 1:da390b3b1330 341 }
juliandekker 1:da390b3b1330 342 else if (externemode == 1 & aanuit == 1){
juliandekker 1:da390b3b1330 343 halt(); ///still todo
347467 0:345f76c72b9a 344 }
juliandekker 1:da390b3b1330 345 /* else {
juliandekker 1:da390b3b1330 346 pc.printf (" hallo \n ");
juliandekker 1:da390b3b1330 347 halt();
juliandekker 1:da390b3b1330 348 }*/
juliandekker 1:da390b3b1330 349 }
juliandekker 1:da390b3b1330 350 }
347467 0:345f76c72b9a 351
347467 0:345f76c72b9a 352
347467 0:345f76c72b9a 353
347467 0:345f76c72b9a 354
347467 0:345f76c72b9a 355