File content as of revision 1:da390b3b1330:

#define DEBUG 0
#include "mbed.h"
#include "rtos.h"
#include "USBHostXpad.h"
#include "utils.h"
#include <cmath>

Serial pc   (USBTX, USBRX);
I2C i2cMod  (p28, p27);
DigitalOut  handmode(p5);                   // hand moet 1 worden (niet connected
DigitalIn   automode(p6);                   // auto
DigitalIn   externemode(p7);                // extern
DigitalIn   aanuit(p8);                     // aan uit
DigitalOut  motorLeftFrontDirAV(p9);       //motor lv 1= vooruit 
DigitalOut  motorRightFrontDirAV(p10);      //MOTOR rv 1= vooruit
DigitalOut  motorLeftRearDirAV(p11);        //motor la 1= vooruit
DigitalOut  motorRightRearDirAV(p12);       //MOTOR ra 1= vooruit        
PwmOut      motorLeftFrontVelAV(p21);       //motor lv
PwmOut      motorRightFrontVelAV (p22);      //Motor rv
PwmOut      motorLeftRearVelAV(p23);        //motor la
PwmOut      motorRightRearVelAV (p24);      //Motor ra
AnalogIn    pot(p19);                       //potmeter
AnalogIn    laserLeft(p15);                 //lasersensorwaarde lv2
AnalogIn    laserRight(p16);                //lasersensorwaarde rv2
DigitalOut  led1(LED1);      
DigitalOut  led2(LED2);      
DigitalOut  led3(LED3);      
DigitalOut  led4(LED4); 
     

float SonarLeft;               // afstandswaarde ultrasoon sensor 1(links?)
float SonarRight;               // afstandswaarde ultrasoon sensor 2 (rechts?)
float AS = 0.5;               // actuele snelheid
float z = 0.5;          // percentage voor het draaien van de motoren
float j = 0.75;         // persentage voor het draaien van de motoren
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
float I = 1.5;          // de afstand vanaf waar de infraroodsensor iets moet detecteren.
float x = 0.5;          // persentage voor het draaien van de motoren
float y = 0.25;         // persentage voor het draaien van de motoren
float motorAcc = 0.3;   // Motor versnelling    pwm per seconde
float motorDec = 0.5;   // Motor vertraging     pwm per seconde
bool motorLeftFrontDirSP = 0;       //motordirection leftfront setpoint
bool motorRightFrontDirSP = 0;      //motordirection leftfront setpoint
bool motorLeftRearDirSP = 0;        //motordirection leftfront setpoint
bool motorRightRearDirSP = 0;       //motordirection leftfront setpoint
float motorLeftFrontVelSP =0;       // motor snelheid leftfront setpoint
float motorRightFrontVelSP =0;       // motor snelheid leftfront setpoint
float motorLeftRearVelSP =0;       // motor snelheid leftfront setpoint
float motorRightRearVelSP =0;       // motor snelheid leftfront setpoint

Xbox360ControllerState controlState;

// Turn right program
void turnRight(){
    led3 = 1; //rechts        
    motorLeftFrontDirSP = 1; //lv
    motorRightFrontDirSP = 0; //rv
    motorLeftRearDirSP = 1; //la
    motorRightRearDirSP = 0; //Ra
    motorLeftFrontVelSP = AS*j;
    motorRightFrontVelSP = AS*z;
    motorLeftRearVelSP = AS*j;
    motorRightRearVelSP = AS*z;
    pc.printf("rechts\r\n");
}

// Turn Left program
void turnLeft(){
    motorLeftFrontDirSP = 1; //lv
    motorRightFrontDirSP = 1; //rv
    motorLeftRearDirSP = 1; //La
    motorRightRearDirSP = 1; //Ra
    motorLeftFrontVelSP = AS*x;
    motorRightFrontVelSP = AS*y;
    motorLeftRearVelSP = AS*x;
    motorRightRearVelSP = AS*y;
    pc.printf("links\r\n");
}

void forward (){
    motorLeftFrontDirSP = 1; //lv
    motorRightFrontDirSP = 1; //rv
    motorLeftRearDirSP = 1; //La
    motorRightRearDirSP = 1; //Ra
    motorLeftFrontVelSP = AS;
    motorRightFrontVelSP = AS;
    motorLeftRearVelSP = AS;
    motorRightRearVelSP = AS;
    pc.printf("rechtsvooruit\r\n");
} 

void reverse(){
    motorLeftFrontDirSP = 0; //lv
    motorRightFrontDirSP = 0; //rv
    motorLeftRearDirSP = 0; //La
    motorRightRearDirSP = 0; //Ra
    motorLeftFrontVelSP = AS;
    motorRightFrontVelSP = AS;
    motorLeftRearVelSP = AS;
    motorRightRearVelSP = AS;
    pc.printf("achteruit\r\n");   
}

void halt(){
    motorLeftFrontDirSP = 0; //lv
    motorRightFrontDirSP = 0; //rv
    motorLeftRearDirSP = 0; //La
    motorRightRearDirSP = 0; //Ra
    motorLeftFrontVelSP = 0;
    motorRightFrontVelSP = 0;
    motorLeftRearVelSP = 0;
    motorRightRearVelSP = 0;
    pc.printf("stop\r\n");   
}
                                                            ///todo nul moet nog stilstaan worden
//Thread MotorController    
void thread_motorControl(void const* arg){
    int controlInterval = 100;// was100
    while(1){
        Thread::wait(controlInterval);
        //MotorLeftFront
        if(motorLeftFrontDirSP == motorLeftFrontDirAV){
            if (motorLeftFrontVelSP > motorLeftFrontVelAV){
                motorLeftFrontVelAV=motorLeftFrontVelAV+(motorAcc/controlInterval*10);
            }
            else if(motorLeftFrontVelSP < motorLeftFrontVelAV){
                motorLeftFrontVelAV=motorLeftFrontVelAV-(motorAcc/controlInterval*10);
            }
        }
        else {
             //regel naar nul
            if ((motorLeftFrontVelAV-(motorDec/controlInterval*10))> 0){
                motorLeftFrontVelAV= motorLeftFrontVelAV-(motorDec/controlInterval*10);
            } 
            else{
                motorLeftFrontDirAV = not motorLeftFrontDirAV; 
            }
        } 
        //MotorRightFront
        if(motorRightFrontDirSP == motorRightFrontDirAV){
            if (motorRightFrontVelSP > motorRightFrontVelAV){
                motorRightFrontVelAV=motorRightFrontVelAV+(motorAcc/controlInterval*10);
            }
            else if(motorRightFrontVelSP < motorRightFrontVelAV){
                motorRightFrontVelAV=motorRightFrontVelAV-(motorAcc/controlInterval*10);
            }
        }
        else {
             //regel naar nul
            if ((motorRightFrontVelAV-(motorDec/controlInterval*10))> 0){
                motorRightFrontVelAV= motorRightFrontVelAV-(motorDec/controlInterval*10);
            } 
            else{
                motorRightFrontDirAV = not motorRightFrontDirAV; 
            }
        }
        //motorLeftRear
        if(motorLeftRearDirSP == motorLeftRearDirAV){
            if (motorLeftRearVelSP > motorLeftRearVelAV){
                motorLeftRearVelAV=motorLeftRearVelAV+(motorAcc/controlInterval*10);
            }
            else if(motorLeftRearVelSP < motorLeftRearVelAV){
                motorLeftRearVelAV=motorLeftRearVelAV-(motorAcc/controlInterval*10);
            }
        }
        else {
             //regel naar nul
            if ((motorLeftRearVelAV-(motorDec/controlInterval*10))> 0){
                motorLeftRearVelAV= motorLeftRearVelAV-(motorDec/controlInterval*10);
            } 
            else{
                motorLeftRearDirAV = not motorLeftRearDirAV; 
            }
        } 
        //motorRightRear
        if(motorRightRearDirSP == motorRightRearDirAV){
            if (motorRightRearVelSP > motorRightRearVelAV){
                motorRightRearVelAV=motorRightRearVelAV+(motorAcc/controlInterval*10);
            }
            else if(motorRightRearVelSP < motorRightRearVelAV){
                motorRightRearVelAV=motorRightRearVelAV-(motorAcc/controlInterval*10);
            }
        }
        else {
             //regel naar nul
            if ((motorRightRearVelAV-(motorDec/controlInterval*10))> 0){
                motorRightRearVelAV= motorRightRearVelAV-(motorDec/controlInterval*10);
            } 
            else{
                motorRightRearDirAV = not motorRightRearDirAV; 
            }
        }  
        pc.printf ("dirAV : %d ", motorLeftFrontDirAV.read());
        pc.printf ("velocityAV :%F", motorLeftFrontVelAV.read()); 
        pc.printf ("dirSP : %d ", motorLeftFrontDirSP);
        pc.printf ("velocitySP :%F\r\n", motorLeftFrontVelSP); 
    }
}    

// programma tbv uitlezen ultasoon sensoren
int i2cAddress1 = 0xF2;
int i2cAddress2 = 0xE0;

void sendStartRangingCommand1(void){
    const char command[] = {0x00, 0x51};
    i2cMod.write(i2cAddress1, command, 2);
}
void sendStartRangingCommand2(void){
    const char command[] = {0x00, 0x51};
    i2cMod.write(i2cAddress2, command, 2);
}
int readRange1(void){
    const char command[]  = {0x02};           //Address of range register
    char response[] = {0x00, 0x00};
    i2cMod.write(i2cAddress1, command, 1, 1);  //Send command
    i2cMod.read(i2cAddress1, response, 2);     //Read 16bits result
    int range = (response[0]<<8)+response[1]; //Shift two bytes into int   
    return range; 
}

int readRange2(void){
    const char command[]  = {0x02};           //Address of range register
    char response[] = {0x00, 0x00};
    i2cMod.write(i2cAddress2, command, 1, 1);  //Send command
    i2cMod.read(i2cAddress2, response, 2);     //Read 16bits result
    int range = (response[0]<<8)+response[1]; //Shift two bytes into int   
    return range; 
}

// Thread Sonar ranging
void thread_sonarControl(void const* arg){
    while (1){
         // sensoren automatisch     
        sendStartRangingCommand1();
        Thread::wait(0.07);
        SonarLeft = readRange1();
        Thread::wait (0.2);
        sendStartRangingCommand2();
        Thread::wait(0.07);
        SonarRight = readRange2();
    }
}

// Programm tbv xbox controller
void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){
    controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
}

// programma tbv xbox controller
void thread_controller(void const* arg){
    USBHostXpad controller;
    controller.attachEvent(&onControlInput);
    
    while(1){    
        // connect programma
        bool wasdisconnected = true;
        //acts as a failsafe
        while(controller.connected()){        
            if(wasdisconnected){            
                controller.led( USBHostXpad::LED1_ON );   
                //stop, wait for controller to reconnect for a second.
                wasdisconnected = false;
            }
            if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ 
            //Stoppen
                halt();
            }        
            //dpad xbox controller
            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){                           
                //vooruit
                pc.printf (" handmatig \n  ");
                if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){
                    forward();
                } 
                //Achteruit
                else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ 
                    reverse();                    
                } 
                //links
                else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ 
                    turnLeft();
                } 
                //rechts
                else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){
                    turnRight();
                }
            }
        }
        //stop, wait for controller to reconnect for a second.
        Thread::wait(500); /// was 500
        controller.connect();
    }
}

//////////////////////////////////////////////////////////////////
int main() {    
    Thread t_controller(thread_controller);
    Thread t_motorControl(thread_motorControl);
    Thread t_sonarControl(thread_sonarControl);    
    while(1) {
        if (aanuit == 1 & automode == 0 & externemode == 0){
            handmode = 1;
            pc.printf (" handmode \r\n  ");
            
        }
        if(aanuit == 1 & automode ==1){   
            //AS =pot*3.3;                  aanzetten weer
            //alles iets detecteerd dus draaien
            if (SonarLeft<=Q && SonarRight<=Q && laserLeft>=I && laserRight>=I){                     
                turnRight();          
            }
            //rv1 rv2 en lv1 iets gedetecteerd hebben, naar links draaien 
            else if (SonarLeft<=Q && SonarRight<=Q && laserLeft<=I && laserRight>=I){       
                turnLeft();   
            }
            //lv1 lv2 en rv1 iets gedetecteerd hebben, naar rechts draaien)
            else if (SonarLeft<=Q && SonarRight<=Q && laserLeft>=I && laserRight<=I){ 
                turnRight();
            }
            //rv1 rv2 iets gedetecteerd hebben, naar links draaien)    
            else if (SonarLeft>Q && SonarRight<Q && laserLeft<I && laserRight>I){
                turnLeft();
            }      
            //lv1 lv2 iets gedetecteerd hebben, naar rechts draaien)
            else if (SonarLeft<Q && SonarRight>Q && laserLeft>I && laserRight<I){
                turnRight();
            }    
            //lv1 rv1 iets gedetecteerd hebben, naar links of rechts draaien)
            else if (SonarLeft<Q && SonarRight>Q && laserLeft<I && laserRight<I){
                if ( laserLeft>=laserRight){
                    turnLeft();
                }
                else if ( laserLeft<=laserRight ){
                    turnRight();
                }
            }
            //lv1 iets gedetecteerd heeft, naar rechts draaien)
            else if (SonarLeft<Q && SonarRight>Q && laserLeft<I && laserRight<I){
                turnRight();
            }
        }
        else if (externemode == 1 & aanuit == 1){
            halt(); ///still todo
        }
       /* else {
            pc.printf (" hallo \n  ");
            halt();
        }*/
    }
}