Jamie Bignell / Mbed 2 deprecated MouseHybridSenseCode

This code enables our mouse to go round the track using the two error methods.

Dependencies:   Nucleo_blink_led mbed

Fork of Nucleo_blink_led by Jamie Bignell

driveFunc.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 driveFunc()
{
    //USE TEMPORARY VARIABLES TO WORK OUT A PULSE LENGTH FOR THE MOTORS, DICTATED BY ERROR OFFSET
    leftMotorValTemp=initialMotorspeed-newPIDs;
    rightMotorValTemp=initialMotorspeed+newPIDs;
    
    //rightMotorValTemp=53.9*exp(2.89*0.001*rightMotorValTemp); 
    //leftMotorValTemp=53.9*exp(2.89*0.001*leftMotorValTemp); 
 
    //IF THE MOTOR SPEED GOES OUTSIDE POSSIBLE BOUNDARIES, CLAMP
    if (leftMotorValTemp>=1000)
    {
        leftMotorValTemp=999;
    }    
    if (rightMotorValTemp>=1000)
    {
        rightMotorValTemp=999;
    }    
    if (leftMotorValTemp<0)
    {
        leftMotorValTemp=0;
    }
    if (rightMotorValTemp<0)
    {
        rightMotorValTemp=0;
    }     
    
    for (int i=0;i<=10;i++)
    {
        if ((leftMotorValTemp>=motorLookup[i][0]) && (leftMotorValTemp<motorLookup[i+1][0]))
        {
            leftMotorValTemp=motorLookup[i][1]+(leftMotorValTemp-motorLookup[i][0])*((motorLookup[i+1][1]-motorLookup[i][1])/(motorLookup[i+1][0]-motorLookup[i][0]));
            break;
        }
    }
    
    for (int i=0;i<=10;i++)
    {
        if ((rightMotorValTemp>=motorLookup[i][0]) && (rightMotorValTemp<motorLookup[i+1][0]))
        {
            rightMotorValTemp=motorLookup[i][1]+(rightMotorValTemp-motorLookup[i][0])*((motorLookup[i+1][1]-motorLookup[i][1])/(motorLookup[i+1][0]-motorLookup[i][0]));
            break;
        }
    }
    
    if (!stopMotors)
    {   
        leftMotorVal.pulsewidth_us(leftMotorValTemp); 
        rightMotorVal.pulsewidth_us(rightMotorValTemp);
    }
    else
    {
        //leftMotorVal.pulsewidth_us(0); 
        //rightMotorVal.pulsewidth_us(0);
        //for(;;){}
    } 

    return;  
}