main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "Servo.h"
00004 //Reads in joystick positions from controller and drives the motors. 
00005 
00006 DigitalOut led1(LED1);
00007 DigitalOut led2(LED2);
00008 DigitalOut led3(LED3);
00009 DigitalOut led4(LED4);
00010 
00011 AnalogIn jx1(p16);
00012 AnalogIn jy1(p15);
00013 //AnalogIn jx2
00014 //AnalogIn jy2
00015 float mot_x = 0.5;
00016 float mot_y = 0.5;
00017 float cam_x = 0;
00018 float cam_y = 0;
00019 
00020 char buf[32];
00021 int idx = 0;
00022 
00023 Serial pc(p9,p10);
00024 //Serial pc(USBTX, USBRX);
00025 
00026 Motor A(p21, p20, p19);  //pwm, inB, inA
00027 Motor B(p22, p17, p12); //pwm, inA, inB
00028 
00029 
00030 int main()
00031 {
00032     pc.baud(115200);
00033     while(1) {
00034             int x = pc.getc();
00035             
00036             if(x == -1) continue;
00037             
00038             //while(!pc.readable());
00039             buf[idx] = (char)x; 
00040             
00041             if(buf[idx] == '\r' || buf[idx] == '\n'){
00042                 sscanf(buf, "%f %f %f %f", &mot_x, &mot_y, &cam_x, &cam_y);
00043                 A.speed(mot_x);
00044                 B.speed(mot_y); 
00045                 idx = 0;
00046             }else{
00047                 idx++;
00048             }
00049     }
00050 }