main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "SBDBT.h"
00004 #define P_OFFSET 10
00005 #define M_OFFSET -10
00006 #define MOTOR_OFFSET 0.0f
00007 sbdbt sb(D1,D0,115200);
00008 PwmOut MotorR_SPD(D3);  PwmOut MotorL_SPD(D4);
00009 PwmOut Motor_R(D5);     PwmOut Motor_L(D6);
00010 DigitalOut MotorR_A(D7);   DigitalOut MotorR_B(D8);
00011 DigitalOut MotorL_A(D9);   DigitalOut MotorL_B(D10);
00012 DigitalOut Motor_R_A(D11); DigitalOut Motor_R_B(D12);
00013 DigitalOut Motor_L_A(D13); DigitalOut Motor_L_B(A0);
00014 DigitalOut Debug_LED(A2);
00015 void move_Motor_R(){
00016     if(sb.JoyStick_dt[3] > P_OFFSET){
00017         MotorR_A = 1; MotorR_B = 0;
00018     }else if(sb.JoyStick_dt[3] < M_OFFSET){
00019         MotorR_A = 0; MotorR_B = 1;
00020     }else{
00021         MotorR_A = 0; MotorR_B = 0;
00022     }    
00023     MotorR_SPD = abs(sb.JoyStick_dt[3])/128.0 + MOTOR_OFFSET;
00024 }
00025 void move_Motor_L(){
00026     if(sb.JoyStick_dt[1] > P_OFFSET){
00027         MotorL_A = 1; MotorL_B = 0;
00028     }else if(sb.JoyStick_dt[1] < M_OFFSET){
00029         MotorL_A = 0; MotorL_B = 1;
00030     }else{
00031         MotorL_A = 0; MotorL_B = 0;
00032     }
00033     MotorL_SPD = abs(sb.JoyStick_dt[1])/128.0 + MOTOR_OFFSET;
00034 }
00035 void Front_Motor(){
00036     if(sb.PS3_L1_bit()){
00037         Motor_R_A = 0; Motor_R_B = 1;
00038         Motor_L_A = 0; Motor_L_B = 1;
00039         Motor_R = 0.8f; Motor_L = 0.8f;
00040     }else if(sb.PS3_L2_bit()){
00041         Motor_R_A = 1; Motor_R_B = 0;
00042         Motor_L_A = 1; Motor_L_B = 0;
00043         Motor_R = 0.8f; Motor_L = 0.8f;
00044     }else {
00045         Motor_R_A = 0; Motor_R_B = 0;
00046         Motor_L_A = 0; Motor_L_B = 0;
00047         Motor_R = 0.0f; Motor_L = 0.0f;
00048     }
00049 }
00050 void setup(){
00051     sb.setup(0);
00052     MotorR_SPD.period(0.00010);
00053     MotorL_SPD.period(0.00010);
00054     Debug_LED = 1;
00055 }
00056 int main(){
00057     setup();
00058     while(1){
00059         Debug_LED = ~Debug_LED;
00060         move_Motor_R();    
00061         move_Motor_L();
00062         Front_Motor();
00063         wait_ms(10);
00064     }
00065 }