Junichi Katsu / Mbed 2 deprecated LineFollow

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

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

Committer:
mbed_Cookbook_SE
Date:
Wed Dec 23 17:03:24 2015 +0000
Revision:
0:7d189fc30328
??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_Cookbook_SE 0:7d189fc30328 1 #include "mbed.h"
mbed_Cookbook_SE 0:7d189fc30328 2 #include "TB6612.h"
mbed_Cookbook_SE 0:7d189fc30328 3
mbed_Cookbook_SE 0:7d189fc30328 4 #define MOTOR_MAX_VALUE 100
mbed_Cookbook_SE 0:7d189fc30328 5
mbed_Cookbook_SE 0:7d189fc30328 6 #define KP 1
mbed_Cookbook_SE 0:7d189fc30328 7 #define KI 0
mbed_Cookbook_SE 0:7d189fc30328 8 #define KD 20
mbed_Cookbook_SE 0:7d189fc30328 9
mbed_Cookbook_SE 0:7d189fc30328 10 BusIn sensor(p15,p16,p17);
mbed_Cookbook_SE 0:7d189fc30328 11 TB6612 MOTOR_A(p21,p19,p20);
mbed_Cookbook_SE 0:7d189fc30328 12 TB6612 MOTOR_B(p22,p29,p30);
mbed_Cookbook_SE 0:7d189fc30328 13 DigitalIn SW1(p25);
mbed_Cookbook_SE 0:7d189fc30328 14 Ticker tick;
mbed_Cookbook_SE 0:7d189fc30328 15
mbed_Cookbook_SE 0:7d189fc30328 16 int motor_cnt_val = 0;
mbed_Cookbook_SE 0:7d189fc30328 17
mbed_Cookbook_SE 0:7d189fc30328 18 void cycle()
mbed_Cookbook_SE 0:7d189fc30328 19 {
mbed_Cookbook_SE 0:7d189fc30328 20 float sensor_val = 0;
mbed_Cookbook_SE 0:7d189fc30328 21 float p_val,d_val,pid_val;
mbed_Cookbook_SE 0:7d189fc30328 22 static float i_val,prev_sensor_val = 0;
mbed_Cookbook_SE 0:7d189fc30328 23
mbed_Cookbook_SE 0:7d189fc30328 24 switch(sensor)
mbed_Cookbook_SE 0:7d189fc30328 25 {
mbed_Cookbook_SE 0:7d189fc30328 26 case 0x01: sensor_val = 100.0; break;
mbed_Cookbook_SE 0:7d189fc30328 27 case 0x03: sensor_val = 50.0; break;
mbed_Cookbook_SE 0:7d189fc30328 28 case 0x02: sensor_val = 0.0; break;
mbed_Cookbook_SE 0:7d189fc30328 29 case 0x06: sensor_val = -50.0; break;
mbed_Cookbook_SE 0:7d189fc30328 30 case 0x04: sensor_val = -100.0; break;
mbed_Cookbook_SE 0:7d189fc30328 31 default: sensor_val = prev_sensor_val; break;
mbed_Cookbook_SE 0:7d189fc30328 32 }
mbed_Cookbook_SE 0:7d189fc30328 33
mbed_Cookbook_SE 0:7d189fc30328 34 p_val = sensor_val;
mbed_Cookbook_SE 0:7d189fc30328 35 i_val += p_val;
mbed_Cookbook_SE 0:7d189fc30328 36 d_val = sensor_val - prev_sensor_val;
mbed_Cookbook_SE 0:7d189fc30328 37 prev_sensor_val = sensor_val;
mbed_Cookbook_SE 0:7d189fc30328 38
mbed_Cookbook_SE 0:7d189fc30328 39 pid_val = (p_val * (KP) ) + (i_val*(KI)) + (d_val*(KD));
mbed_Cookbook_SE 0:7d189fc30328 40
mbed_Cookbook_SE 0:7d189fc30328 41 motor_cnt_val = (int)(pid_val);
mbed_Cookbook_SE 0:7d189fc30328 42 }
mbed_Cookbook_SE 0:7d189fc30328 43
mbed_Cookbook_SE 0:7d189fc30328 44 int main() {
mbed_Cookbook_SE 0:7d189fc30328 45 SW1.mode(PullUp);
mbed_Cookbook_SE 0:7d189fc30328 46
mbed_Cookbook_SE 0:7d189fc30328 47 tick.attach(cycle,0.01);
mbed_Cookbook_SE 0:7d189fc30328 48
mbed_Cookbook_SE 0:7d189fc30328 49 while(SW1==1);
mbed_Cookbook_SE 0:7d189fc30328 50
mbed_Cookbook_SE 0:7d189fc30328 51 wait(1);
mbed_Cookbook_SE 0:7d189fc30328 52
mbed_Cookbook_SE 0:7d189fc30328 53 while(1) {
mbed_Cookbook_SE 0:7d189fc30328 54 if( motor_cnt_val < 0 ) {
mbed_Cookbook_SE 0:7d189fc30328 55 MOTOR_A = MOTOR_MAX_VALUE
mbed_Cookbook_SE 0:7d189fc30328 56 + motor_cnt_val;
mbed_Cookbook_SE 0:7d189fc30328 57 } else {
mbed_Cookbook_SE 0:7d189fc30328 58 MOTOR_A = MOTOR_MAX_VALUE;
mbed_Cookbook_SE 0:7d189fc30328 59 }
mbed_Cookbook_SE 0:7d189fc30328 60 if( motor_cnt_val > 0 ) {
mbed_Cookbook_SE 0:7d189fc30328 61 MOTOR_B = MOTOR_MAX_VALUE
mbed_Cookbook_SE 0:7d189fc30328 62 - motor_cnt_val;
mbed_Cookbook_SE 0:7d189fc30328 63 } else {
mbed_Cookbook_SE 0:7d189fc30328 64 MOTOR_B = MOTOR_MAX_VALUE;
mbed_Cookbook_SE 0:7d189fc30328 65 }
mbed_Cookbook_SE 0:7d189fc30328 66 }
mbed_Cookbook_SE 0:7d189fc30328 67 }