File content as of revision 6:59f239c83de4:

//
//  drive.cpp
//  MOTORDRIVER
//
//  Created by Antoine Laurens on 15.03.16.
//  Copyright (c) 2016 Antoine Laurens. All rights reserved.
//

#include "drive.h"


void down(LOCALIZE_xya *xya,int stopPoint,float *pwmDuty1,float *pwmDuty2)
{
    driveStraightD(xya,stopPoint,1,pwmDuty1,pwmDuty2);
    driveStraightD(xya,xya->y-sque_size/2-20, 0,pwmDuty1,pwmDuty2);
}

void driveStraightD(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2)
{
    //set direction bit so robot goes backward or forwards
    
    *pwmDuty1=0.1;
    *pwmDuty2=0.1;    
    if (dir==1)
    {
        while (xya->y<stopPoint)
        { 
            if(abs(xya->y-stopPoint)<15)
            {
                //set pwm to k*dist so it slows down
            }          
        }
    }
    else 
    {
        while (xya->y>stopPoint)
        {    
            if(abs(xya->y-stopPoint)<15)
            {
                //set pwm to k*dist so it slows down
            }      
        }
    }
    
    *pwmDuty1=0;
    *pwmDuty2=0;
 
}

void driveStraightS(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2)
{
    //set direction bit so robot goes backward or forwards
    
    *pwmDuty1=0.1;
    *pwmDuty2=0.1;
    
    if (xya->x>win_w/2)
    {
        while (xya->x>stopPoint)
        {
            if(abs(xya->y-stopPoint)<15)
            {
                //set pwm to k*dist so it slows down
            }          
        }
    } 
    else
    {
        while (xya->x<stopPoint)
        {
            if(abs(xya->y-stopPoint)<15)
            {
                //set pwm to k*dist so it slows down
            }          
        }
    }   
    *pwmDuty1=0;
    *pwmDuty2=0; 
}

void rotate(int stopAngle,LOCALIZE_xya *xya,float *pwmDuty1,float *pwmDuty2)
{
    int currAngle;
    int initAngle=xya->a;
    int a=abs(initAngle-stopAngle)<abs(initAngle-stopAngle+359);
    int b=initAngle<stopAngle;

    switch(a<<1+b)
    {
        case 0:  //turn right with wrap around
        
            //robot must turn right set pwm for that at constant rate
            
            currAngle=xya->a;
            /* don't put directly xya-> in the if statement because it could 
            change after I modify and before the beginning of the if statement*/
            while(currAngle<stopAngle+359)
            {
                currAngle=xya->a;        
                if (currAngle<stopAngle)
                {
                    currAngle=currAngle+359;
                }
                
                if (abs(currAngle-stopAngle+359)<15)
                {
                    //set pwm to proprtionat value 
                    //k*currAngl so that it slows down 
                }
            }
        break;

        case 1://turn left no wrap around
            //set pwm so the robot starts turning left on both
            while(xya->a>stopAngle)
            {
                if (abs(xya->a-stopAngle)<15)
                {
                    //set pwm to k*xya->a to slow it down
                }    
            }    
        break;
        
        case 2: //left wrap around
            //set pwm to go left at const rate
        
            currAngle=xya->a;
            /* don't put directly xya-> in the if statement because it could 
            change after I modify and before the beginning of the if statement*/
            while(currAngle+359>stopAngle)
            {
                currAngle=xya->a;        
                if (currAngle>stopAngle)
                {
                    currAngle=currAngle-359;
                }
                
                if (abs(currAngle-stopAngle+359)<15)
                {
                    //set pwm to proprtionat value 
                    //k*currAngl so that it slows down 
                }
            }
        break;

        case 3: //turn right no wrap around
            //set pwm so the robot starts turning right on both
            while(xya->a<stopAngle)
            {
                if (abs(xya->a-stopAngle)<15)
                {
                    //set pwm to k*xya->a to slow it down
                }    
            }    
        break;
    }
    //set pwm =0 so the robot stops
        
}