test

Dependencies:   mbed

Dependents:   MouseHybridSenseCode

calibrateFunc.cpp

Committer:
JamieBignell
Date:
2018-04-28
Revision:
7:11dd5581c763
Parent:
6:a652deaae134

File content as of revision 7:11dd5581c763:

#include <mbed.h>
#include <funcdef.h>

void CalibrateFunc()
{
    //USER PUTS MOUSE LEFT OF LINE 
    
    //TAKE MIN/MAX VALUES 1ST TIME
    for (int i=0;i<=7;i++)
    {
        sensorValue[i]=((sensorPin[i]).read_u16()>>4); 
        sensorMinValue1[i]=sensorValue[i];
        sensorMaxValue1[i]=sensorValue[i];
    }
    
    //LEFT WHEEL STARTS TURN      
    leftMotorVal.pulsewidth_us(calibrationMotorspeed); 
    rightMotorVal.pulsewidth_us(0);  
    
    //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET AND A SMIDGE MORE
    /////////////////////////////////////////////       
    int doneYet[] = {0,0,0,0,0,0,0,0};
    while(1)
    {    
        for (int i=0;i<=7;i++)
        {
            sensorValue[i]=((sensorPin[i]).read_u16()>>4);      
            if (sensorValue[i]>=sensorMaxValue1[i])
            {    
                sensorMaxValue1[i]=sensorValue[i];      
            }
            else if ((sensorValue[i]+250)<=sensorMaxValue1[i]) 
            {
                doneYet[i]=1;                
                if (((doneYet[6]) == 1) && ((doneYet[4]) == 1) && ((doneYet[2]) == 1) && ((doneYet[0]) == 1) && ((doneYet[1]) == 1) && ((doneYet[3]) == 1) && ((doneYet[5]) == 1) && ((doneYet[7]) == 1))
                {
                    goto endOfScan1;
                }
            }
        }            
    }
    endOfScan1:
    wait_ms(50);
    /////////////////////////////////////////////
  
    //STOP WHEELS
    leftMotorVal.pulsewidth_us(0); 
    rightMotorVal.pulsewidth_us(0); 
    
    //TURN LED OFF    
    LEDVal=0; 
    
    wait_ms(500);
      
    //TAKE MIN/MAX VALUES 2ND TIME
    for (int i=0;i<=7;i++)
    {
        sensorValue[i]=((sensorPin[i]).read_u16()>>4);
        sensorMinValue2[i]=sensorValue[i];
        sensorMaxValue2[i]=sensorValue[i];
    }
    
    //RIGHT WHEEL STARTS TURN  
    leftMotorVal.pulsewidth_us(0); 
    rightMotorVal.pulsewidth_us(calibrationMotorspeed);  
  
    //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET IN NEW VARIABLES
    int doneYet2[] = {0,0,0,0,0,0,0,0};
    while(1)
    {    
        for (int i=0;i<=7;i++)
        {
            sensorValue[i]=((sensorPin[i]).read_u16()>>4);      
            if (sensorValue[i]>=sensorMaxValue2[i])
            {    
                sensorMaxValue2[i]=sensorValue[i];      
            }
            else if ((sensorValue[i]+250)<=sensorMaxValue2[i]) 
            {   
                doneYet2[i]=1;                
                if (((doneYet2[6]) == 1) && ((doneYet2[4]) == 1) && ((doneYet2[2]) == 1) && ((doneYet2[0]) == 1) && ((doneYet2[1]) == 1) && ((doneYet2[3]) == 1) && ((doneYet2[5]) == 1) && ((doneYet2[7]) == 1))
                {
                    goto endOfScan2;
                }
            }
        }           
    }
    endOfScan2:
    
    wait_ms(50);
    ///////////////////////////////////////////// 
    
    //STOP WHEELS
    leftMotorVal.pulsewidth_us(0); 
    rightMotorVal.pulsewidth_us(0);
    
    wait_ms(250); 
    
    //AVERAGE THESE VALUES       
    for (int i=0;i<=7;i++)
    {
        sensorMinValue[i]=(sensorMinValue1[i]+sensorMinValue2[i])/2;
        sensorMaxValue[i]=(sensorMaxValue1[i]+sensorMaxValue2[i])/2;
    }  
    
    //SET THRESHOLDS FOR ERROR OF 0
    for (int i=0;i<=7;i++)
    {
        sensorThreshold[i]=sensorMaxValue[i]-250;
    } 
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////     
    return;
}