main.cpp

00001 #include "mbed.h"
00002 #include "motor.h"
00003 #include <ros.h>
00004 #include <std_msgs/Float64.h>
00005 #include <std_msgs/Int32.h>
00006 #include <std_msgs/String.h>
00007 #include <geometry_msgs/Point.h>
00008 
00009 
00010 
00011 BusOut bus(D11,D12);
00012 MotorCtl motor1(D3,bus,D4,D5);
00013 InterruptIn tachoINT1(D4);
00014 InterruptIn tachoINT2(D5);
00015 RawSerial pc(USBTX,USBRX,115200);
00016 
00017 char rx_buffer[10];
00018 int index=0;
00019 volatile int flag;
00020 void update_current(void)
00021 {
00022     motor1.UpdateCurrentPosition();
00023     //  pc.printf("Update Position\r\n");
00024 }
00025 void rx_cb(void)
00026 {
00027     char ch;
00028     ch = pc.getc();
00029     pc.putc(ch);
00030     rx_buffer[index++]=ch;
00031     if(ch==0x0D) {
00032         pc.putc(0x0A);
00033         rx_buffer[--index]='\0';
00034         index=0;
00035         flag=1;
00036     }
00037 }
00038 
00039 void set_speed(void)
00040 {
00041     int speed;
00042     speed = atoi((const char*)rx_buffer);
00043 
00044     motor1.setTarget(speed);
00045 
00046    pc.printf(" Set speed = %d\r\n", speed);
00047 
00048 }
00049 
00050 int main()
00051 {
00052 
00053     pc.attach(callback(rx_cb));
00054     int rpm;
00055     tachoINT1.fall(&update_current);
00056     tachoINT1.rise(&update_current);
00057     tachoINT2.fall(&update_current);
00058     tachoINT2.rise(&update_current);
00059 
00060 //    char flaw_curr_state[10] = "stable";
00061     wait(1);
00062     motor1.setTarget(60);
00063     
00064     while(1) {
00065         flag=0;
00066 
00067         pc.printf("Enter the value for speed [-78,78]\r\n");
00068         while(flag!=1){
00069             rpm = motor1.getRPM();
00070 //            pc.printf("RPM: %d\r\n",rpm);
00071             wait(1);
00072         }
00073 //        motor1.setTarget(60);
00074         set_speed();
00075     }
00076 
00077 }