main.cpp

00001 #include "mbed.h"
00002 //#include <math.h>
00003 
00004 AnalogIn analog_value_0(A0);
00005 AnalogIn analog_value_1(A1);
00006 
00007 PwmOut left(PWM_OUT);
00008 PwmOut right(PB_5);
00009 
00010 DigitalIn mybutton(USER_BUTTON);
00011 DigitalOut myled(LED1);
00012 
00013 // Calculate the corresponding acquisition measure for a given value in mV
00014 #define MV(x) ((0xFFF*x)/3300)
00015 #define scale 0.20  //unit %
00016 #define speed_range 1000.0
00017 #define dirction_range 11.0
00018 
00019 int main()
00020 {
00021     char state_motor=0;
00022     //int count
00023     left.period(0.01);
00024     right.period(0.01);
00025 
00026     while(1) {
00027         uint16_t speed = analog_value_0.read_u16(); // Converts and read the analog input value
00028         uint16_t dirction = analog_value_1.read_u16(); // Converts and read the analog input value
00029 
00030         speed = ((speed*(speed_range+1))/65535);
00031         dirction =((dirction*(dirction_range+1))/65535);
00032 
00033         if(speed>speed_range)speed=speed_range;
00034         if(dirction>dirction_range)dirction=dirction_range;
00035         
00036         int16_t dirction_0 = dirction-(dirction_range/2);
00037 
00038         printf("Speed %d\n",speed);
00039         printf("Dirction %d  \n",dirction_0);
00040 
00041         if (mybutton == 0) { // Button is pressed
00042             wait_ms(10);
00043             state_motor = !state_motor;
00044         }
00045 
00046         if(state_motor ==0) {
00047             left.write(0.0);
00048             right.write(0.0);
00049         } else {
00050             if(dirction_0 >= 0) {
00051                 left.write((speed - (scale*speed*dirction_0))/speed_range);
00052                 right.write(speed/speed_range);
00053             } else {
00054                 right.write((speed + (scale*speed*dirction_0))/speed_range);
00055                 left.write(speed/speed_range);
00056             }
00057         }
00058     }
00059 }