maze competition 2014 at nagoya

Dependencies:   mbed

main.cpp

Committer:
soulx
Date:
2015-12-23
Revision:
0:fab561035537

File content as of revision 0:fab561035537:

#include "mbed.h"
//#include <math.h>

AnalogIn analog_value_0(A0);
AnalogIn analog_value_1(A1);

PwmOut left(PWM_OUT);
PwmOut right(PB_5);

DigitalIn mybutton(USER_BUTTON);
DigitalOut myled(LED1);

// Calculate the corresponding acquisition measure for a given value in mV
#define MV(x) ((0xFFF*x)/3300)
#define scale 0.20  //unit %
#define speed_range 1000.0
#define dirction_range 11.0

int main()
{
    char state_motor=0;
    //int count
    left.period(0.01);
    right.period(0.01);

    while(1) {
        uint16_t speed = analog_value_0.read_u16(); // Converts and read the analog input value
        uint16_t dirction = analog_value_1.read_u16(); // Converts and read the analog input value

        speed = ((speed*(speed_range+1))/65535);
        dirction =((dirction*(dirction_range+1))/65535);

        if(speed>speed_range)speed=speed_range;
        if(dirction>dirction_range)dirction=dirction_range;
        
        int16_t dirction_0 = dirction-(dirction_range/2);

        printf("Speed %d\n",speed);
        printf("Dirction %d  \n",dirction_0);

        if (mybutton == 0) { // Button is pressed
            wait_ms(10);
            state_motor = !state_motor;
        }

        if(state_motor ==0) {
            left.write(0.0);
            right.write(0.0);
        } else {
            if(dirction_0 >= 0) {
                left.write((speed - (scale*speed*dirction_0))/speed_range);
                right.write(speed/speed_range);
            } else {
                right.write((speed + (scale*speed*dirction_0))/speed_range);
                left.write(speed/speed_range);
            }
        }
    }
}