Pipe Inpection Robot / Mbed OS GPG_motor

V1

Dependencies:   BufferedSerial sn7544

Fork of GPG_motor by Kang mingyo

main.cpp@13:681f3d1e9077, 2019-08-07 (annotated)

Committer:
kangmingyo
Date:
Wed Aug 07 13:40:22 2019 +0000
Revision:
13:681f3d1e9077
Parent:
11:2228e8931266 
0;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kangmingyo 0:dde6e4d8c301 1 #include "mbed.h"
kangmingyo 1:1d18a2e8ce45 2 #include "motor.h"
Jeonghoon 7:13dd93a0efe8 3 #include <ros.h>
Jeonghoon 7:13dd93a0efe8 4 #include <std_msgs/Float64.h>
Jeonghoon 7:13dd93a0efe8 5 #include <std_msgs/Int32.h>
Jeonghoon 8:efe5a5b1f10a 6 #include <std_msgs/String.h>
Jeonghoon 11:2228e8931266 7 #include <geometry_msgs/Point.h>
Jeonghoon 7:13dd93a0efe8 8
Jeonghoon 7:13dd93a0efe8 9
Jeonghoon 7:13dd93a0efe8 10
kangmingyo 13:681f3d1e9077 11 BusOut bus(D11,D12);
kangmingyo 13:681f3d1e9077 12 MotorCtl motor1(D3,bus,D4,D5);
kangmingyo 1:1d18a2e8ce45 13 InterruptIn tachoINT1(D4);
kangmingyo 1:1d18a2e8ce45 14 InterruptIn tachoINT2(D5);
kangmingyo 13:681f3d1e9077 15 RawSerial pc(USBTX,USBRX,115200);
Jeonghoon 7:13dd93a0efe8 16
kangmingyo 2:a04440f0d001 17 char rx_buffer[10];
kangmingyo 2:a04440f0d001 18 int index=0;
kangmingyo 2:a04440f0d001 19 volatile int flag;
kangmingyo 2:a04440f0d001 20 void update_current(void)
kangmingyo 2:a04440f0d001 21 {
kangmingyo 1:1d18a2e8ce45 22 motor1.UpdateCurrentPosition();
kangmingyo 2:a04440f0d001 23 // pc.printf("Update Position\r\n");
kangmingyo 2:a04440f0d001 24 }
kangmingyo 13:681f3d1e9077 25 void rx_cb(void)
kangmingyo 13:681f3d1e9077 26 {
kangmingyo 13:681f3d1e9077 27 char ch;
kangmingyo 13:681f3d1e9077 28 ch = pc.getc();
kangmingyo 13:681f3d1e9077 29 pc.putc(ch);
kangmingyo 13:681f3d1e9077 30 rx_buffer[index++]=ch;
kangmingyo 13:681f3d1e9077 31 if(ch==0x0D) {
kangmingyo 13:681f3d1e9077 32 pc.putc(0x0A);
kangmingyo 13:681f3d1e9077 33 rx_buffer[--index]='\0';
kangmingyo 13:681f3d1e9077 34 index=0;
kangmingyo 13:681f3d1e9077 35 flag=1;
kangmingyo 13:681f3d1e9077 36 }
kangmingyo 13:681f3d1e9077 37 }
kangmingyo 13:681f3d1e9077 38
kangmingyo 13:681f3d1e9077 39 void set_speed(void)
kangmingyo 13:681f3d1e9077 40 {
kangmingyo 13:681f3d1e9077 41 int speed;
kangmingyo 13:681f3d1e9077 42 speed = atoi((const char*)rx_buffer);
kangmingyo 13:681f3d1e9077 43
kangmingyo 13:681f3d1e9077 44 motor1.setTarget(speed);
kangmingyo 13:681f3d1e9077 45
kangmingyo 13:681f3d1e9077 46 pc.printf(" Set speed = %d\r\n", speed);
kangmingyo 13:681f3d1e9077 47
kangmingyo 13:681f3d1e9077 48 }
kangmingyo 0:dde6e4d8c301 49
kangmingyo 2:a04440f0d001 50 int main()
kangmingyo 2:a04440f0d001 51 {
Jeonghoon 11:2228e8931266 52
kangmingyo 13:681f3d1e9077 53 pc.attach(callback(rx_cb));
kangmingyo 13:681f3d1e9077 54 int rpm;
kangmingyo 1:1d18a2e8ce45 55 tachoINT1.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 56 tachoINT1.rise(&update_current);
kangmingyo 1:1d18a2e8ce45 57 tachoINT2.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 58 tachoINT2.rise(&update_current);
kangmingyo 13:681f3d1e9077 59
Jeonghoon 8:efe5a5b1f10a 60 // char flaw_curr_state[10] = "stable";
kangmingyo 13:681f3d1e9077 61 wait(1);
kangmingyo 13:681f3d1e9077 62 motor1.setTarget(60);
Jeonghoon 8:efe5a5b1f10a 63
kangmingyo 2:a04440f0d001 64 while(1) {
kangmingyo 2:a04440f0d001 65 flag=0;
kangmingyo 5:e6f9768e1438 66
kangmingyo 13:681f3d1e9077 67 pc.printf("Enter the value for speed [-78,78]\r\n");
kangmingyo 13:681f3d1e9077 68 while(flag!=1){
kangmingyo 4:a72f75611198 69 rpm = motor1.getRPM();
kangmingyo 13:681f3d1e9077 70 // pc.printf("RPM: %d\r\n",rpm);
kangmingyo 2:a04440f0d001 71 wait(1);
kangmingyo 1:1d18a2e8ce45 72 }
kangmingyo 3:0c6258635590 73 // motor1.setTarget(60);
kangmingyo 13:681f3d1e9077 74 set_speed();
kangmingyo 2:a04440f0d001 75 }
kangmingyo 0:dde6e4d8c301 76
Jeonghoon 8:efe5a5b1f10a 77 }