syudouki.cpp

00001 #include "mbed.h"
00002 #include "kbt.h"
00003 
00004 KBT kbt(PA_9,PA_10);
00005 Serial pc(USBTX,USBRX);
00006 
00007 PwmOut rifrpl (PC_12);
00008 PwmOut rifrmi (PA_13);
00009 PwmOut ribapl (PA_14);
00010 PwmOut ribami (PA_15);
00011 PwmOut lefrpl (PB_7);
00012 PwmOut lefrmi (PC_13);
00013 PwmOut lebapl (PF_0);
00014 PwmOut lebami (PC_2);
00015 PwmOut servo (PC_3);
00016 DigitalOut air (PB_10);
00017 
00018 double map(double x, double in_min, double in_max, double out_min, double out_max)
00019 {
00020   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00021 }
00022 
00023 double Abs ( double value ){
00024     if ( value < 0 )
00025       value = value*(-1);
00026     return value;
00027     }
00028 
00029 int main() {
00030     
00031     kbt.init(2400);
00032     pc.baud(9600);
00033     
00034     while(1) {
00035         double Larpow = kbt.Stick[L_around];
00036         double Luppow = kbt.Stick[L_updown];
00037         
00038         int dir;
00039         
00040         double pow = 0;
00041         
00042         double Larpowabs = Abs(Larpow);
00043         double Luppowabs = Abs(Luppow);
00044         
00045         if (Larpowabs > Luppowabs){
00046            if (Larpow < 0){
00047               dir = 0;
00048               pow = Larpow*(-1);
00049               }
00050            else if (Larpow > 0){
00051               dir = 1;
00052               pow = Larpow;
00053               }
00054         }      
00055         else {
00056             if (Luppow < 0){
00057                 dir = 2;
00058                 pow = Luppow*(-1);
00059                 } 
00060             else if (Luppow > 0){
00061                 dir = 3;
00062                 pow = Luppow;
00063                 }
00064         }        
00065         
00066         pow = map(pow,0.0,255.0,0.0,1.0);
00067         
00068         switch (dir){
00069             case 0 :
00070                rifrpl = 0;
00071                rifrmi = pow;
00072                ribapl = pow;
00073                ribami = 0;
00074                lefrpl = pow;
00075                lefrmi = 0;
00076                lebapl = 0;
00077                lebami = pow;
00078                break;
00079             case 1 :
00080                rifrpl = pow;
00081                rifrmi = 0;
00082                ribapl = 0;
00083                ribami = pow;
00084                lefrpl = 0;
00085                lefrmi = pow;
00086                lebapl = pow;
00087                lebami = 0;
00088                break;
00089             case 2 :
00090                rifrpl = 0;
00091                rifrmi = pow;
00092                ribapl = 0;
00093                ribami = pow;
00094                lefrpl = 0;
00095                lefrmi = pow;
00096                lebapl = 0;
00097                lebami = pow;
00098                break;
00099             case 3 :
00100                rifrpl = pow;
00101                rifrmi = 0;
00102                ribapl = pow;
00103                ribami = 0;
00104                lefrpl = pow;
00105                lefrmi = 0;
00106                lebapl = pow;
00107                lebami = 0;
00108                break;
00109         }
00110         
00111         bool ser = kbt.Button[circle];
00112         servo.period_ms(20);
00113         if (ser == 1){
00114            servo.pulsewidth_us(700);
00115            wait(8);
00116            servo.pulsewidth_us(1500);
00117            wait(8);
00118            }
00119         
00120         bool ai = kbt.Button[triangle];
00121         if (ai == 1)
00122            air = 1;
00123         else
00124            air = 0;
00125      }
00126 }