main.cpp

00001 #include "mbed.h"
00002 #include "EC.h"
00003 #include "math.h"
00004 
00005 PwmOut motor1F(PC_9);
00006 PwmOut motor1B(PC_8);
00007 PwmOut motor2F(PC_7);
00008 PwmOut motor2B(PC_6);
00009 Ec ec1(PA_12, PA_11, NC, 500, 0.05);
00010 Ec ec2(PC_4, PB_14, NC, 500, 0.05);
00011 Ticker ticker;
00012 int tag1 = -100;
00013 int tag2 = 100;
00014 int gap1;
00015 int gap2;
00016 int a=0;
00017 int i=1;
00018 
00019 void calomega() {
00020   ec1.CalOmega();
00021   ec2.CalOmega();
00022   a=a+1;
00023   if(a>=30&&i==1){
00024       tag1=-400;
00025       a=0;
00026       i=0;
00027       }
00028       if(a>=30&&i==0){
00029           tag1=-100;
00030           a=0;
00031           i=1;
00032           }}
00033 
00034 int main() {
00035   ticker.attach(&calomega, 0.05);
00036   motor1F.period_us(50);
00037   motor1B.period_us(50);
00038   motor2F.period_us(50);
00039   motor2B.period_us(50);
00040   while (1) {
00041     printf("count=%d\r\n", ec1.getCount());
00042     gap1 = tag1 - ec1.getCount();
00043     gap2 = tag2 - ec2.getCount();
00044     if (gap1 > 0) {
00045       if (gap1 > 50) {
00046         motor1F = 0;
00047         motor1B = 0.3;
00048       }
00049       if (gap1 < 50) {
00050         motor1F = 0;
00051         motor1B = gap1 * 0.005;
00052       }
00053     }
00054     if (gap1 < 0) {
00055       if (gap1 < -50) {
00056         motor1F = 0.3;
00057         motor1B = 0;
00058       }
00059       if (gap1 > -50) {
00060         motor1F = -gap1 * 0.005;
00061         motor1B = 0;
00062       }
00063     }
00064   }
00065 }
00066