d

Dependencies:   Motor Servo mbed

main.cpp

Committer:
iemretep
Date:
2017-04-21
Revision:
0:1f5a1332637b

File content as of revision 0:1f5a1332637b:

#include "mbed.h"
#include "Motor.h"
#include "Servo.h"
//Reads in joystick positions from controller and drives the motors. 

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

AnalogIn jx1(p16);
AnalogIn jy1(p15);
//AnalogIn jx2
//AnalogIn jy2
float mot_x = 0.5;
float mot_y = 0.5;
float cam_x = 0;
float cam_y = 0;

char buf[32];
int idx = 0;

Serial pc(p9,p10);
//Serial pc(USBTX, USBRX);

Motor A(p21, p20, p19);  //pwm, inB, inA
Motor B(p22, p17, p12); //pwm, inA, inB


int main()
{
    pc.baud(115200);
    while(1) {
            int x = pc.getc();
            
            if(x == -1) continue;
            
            //while(!pc.readable());
            buf[idx] = (char)x; 
            
            if(buf[idx] == '\r' || buf[idx] == '\n'){
                sscanf(buf, "%f %f %f %f", &mot_x, &mot_y, &cam_x, &cam_y);
                A.speed(mot_x);
                B.speed(mot_y); 
                idx = 0;
            }else{
                idx++;
            }
    }
}