potiwat ngamkajornwiwat / Mbed 2 deprecated Micro_robot_1cm_mk

maze competition 2014 at nagoya

Dependencies:   mbed

main.cpp@0:fab561035537, 2015-12-23 (annotated)

Committer:
soulx
Date:
Wed Dec 23 12:19:49 2015 +0000
Revision:
0:fab561035537
0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:fab561035537 1 #include "mbed.h"
soulx 0:fab561035537 2 //#include <math.h>
soulx 0:fab561035537 3
soulx 0:fab561035537 4 AnalogIn analog_value_0(A0);
soulx 0:fab561035537 5 AnalogIn analog_value_1(A1);
soulx 0:fab561035537 6
soulx 0:fab561035537 7 PwmOut left(PWM_OUT);
soulx 0:fab561035537 8 PwmOut right(PB_5);
soulx 0:fab561035537 9
soulx 0:fab561035537 10 DigitalIn mybutton(USER_BUTTON);
soulx 0:fab561035537 11 DigitalOut myled(LED1);
soulx 0:fab561035537 12
soulx 0:fab561035537 13 // Calculate the corresponding acquisition measure for a given value in mV
soulx 0:fab561035537 14 #define MV(x) ((0xFFF*x)/3300)
soulx 0:fab561035537 15 #define scale 0.20 //unit %
soulx 0:fab561035537 16 #define speed_range 1000.0
soulx 0:fab561035537 17 #define dirction_range 11.0
soulx 0:fab561035537 18
soulx 0:fab561035537 19 int main()
soulx 0:fab561035537 20 {
soulx 0:fab561035537 21 char state_motor=0;
soulx 0:fab561035537 22 //int count
soulx 0:fab561035537 23 left.period(0.01);
soulx 0:fab561035537 24 right.period(0.01);
soulx 0:fab561035537 25
soulx 0:fab561035537 26 while(1) {
soulx 0:fab561035537 27 uint16_t speed = analog_value_0.read_u16(); // Converts and read the analog input value
soulx 0:fab561035537 28 uint16_t dirction = analog_value_1.read_u16(); // Converts and read the analog input value
soulx 0:fab561035537 29
soulx 0:fab561035537 30 speed = ((speed*(speed_range+1))/65535);
soulx 0:fab561035537 31 dirction =((dirction*(dirction_range+1))/65535);
soulx 0:fab561035537 32
soulx 0:fab561035537 33 if(speed>speed_range)speed=speed_range;
soulx 0:fab561035537 34 if(dirction>dirction_range)dirction=dirction_range;
soulx 0:fab561035537 35
soulx 0:fab561035537 36 int16_t dirction_0 = dirction-(dirction_range/2);
soulx 0:fab561035537 37
soulx 0:fab561035537 38 printf("Speed %d\n",speed);
soulx 0:fab561035537 39 printf("Dirction %d \n",dirction_0);
soulx 0:fab561035537 40
soulx 0:fab561035537 41 if (mybutton == 0) { // Button is pressed
soulx 0:fab561035537 42 wait_ms(10);
soulx 0:fab561035537 43 state_motor = !state_motor;
soulx 0:fab561035537 44 }
soulx 0:fab561035537 45
soulx 0:fab561035537 46 if(state_motor ==0) {
soulx 0:fab561035537 47 left.write(0.0);
soulx 0:fab561035537 48 right.write(0.0);
soulx 0:fab561035537 49 } else {
soulx 0:fab561035537 50 if(dirction_0 >= 0) {
soulx 0:fab561035537 51 left.write((speed - (scale*speed*dirction_0))/speed_range);
soulx 0:fab561035537 52 right.write(speed/speed_range);
soulx 0:fab561035537 53 } else {
soulx 0:fab561035537 54 right.write((speed + (scale*speed*dirction_0))/speed_range);
soulx 0:fab561035537 55 left.write(speed/speed_range);
soulx 0:fab561035537 56 }
soulx 0:fab561035537 57 }
soulx 0:fab561035537 58 }
soulx 0:fab561035537 59 }