TurtleBot / Mbed 2 deprecated Servo_without_thread

Servo code v.1

Dependencies:   Servo mbed

main.cpp@1:852156b5cca1, 2018-04-02 (annotated)

Committer:
59010050
Date:
Mon Apr 02 10:18:44 2018 +0000
Revision:
1:852156b5cca1
Parent:
0:3af6fae90816 
latest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khanchana 0:3af6fae90816 1 #include "mbed.h"
Khanchana 0:3af6fae90816 2 #include "Servo.h"
Khanchana 0:3af6fae90816 3 #include "rtos.h"
59010050 1:852156b5cca1 4 #include "attitude.h"
Khanchana 0:3af6fae90816 5
Khanchana 0:3af6fae90816 6 Serial pc(USBTX, USBRX);
59010050 1:852156b5cca1 7 //Serial bt(A7,A2);
59010050 1:852156b5cca1 8 Timer timer1;
59010050 1:852156b5cca1 9 Thread thread1;
59010050 1:852156b5cca1 10
59010050 1:852156b5cca1 11 Thread thread2;
59010050 1:852156b5cca1 12
59010050 1:852156b5cca1 13 void IMU()
59010050 1:852156b5cca1 14 {
59010050 1:852156b5cca1 15 while(1) {
59010050 1:852156b5cca1 16 if (timer1.read_us() >=1000)// read time in ms
59010050 1:852156b5cca1 17 {
59010050 1:852156b5cca1 18 attitude_get();
59010050 1:852156b5cca1 19 pc.printf(" %f \t", ax*10 );
59010050 1:852156b5cca1 20 pc.printf(" %f \t", ay*10 );
59010050 1:852156b5cca1 21 pc.printf(" %f \t", az*10 - 10); //cm/s*s
59010050 1:852156b5cca1 22
59010050 1:852156b5cca1 23 pc.printf(" %f \t", gx );
59010050 1:852156b5cca1 24 pc.printf(" %f \t", gy );
59010050 1:852156b5cca1 25 pc.printf(" %f \t", gz ); //deg/s */
59010050 1:852156b5cca1 26 pc.printf("%.0f\t %.0f \t \n\r", roll, pitch );
59010050 1:852156b5cca1 27
59010050 1:852156b5cca1 28 timer1.reset(); // reset timer
59010050 1:852156b5cca1 29 }
59010050 1:852156b5cca1 30 }
59010050 1:852156b5cca1 31 }
59010050 1:852156b5cca1 32
Khanchana 0:3af6fae90816 33 Servo Servo1(D6);
Khanchana 0:3af6fae90816 34 Servo Servo2(D8);
Khanchana 0:3af6fae90816 35 Servo Servo3(D9);
Khanchana 0:3af6fae90816 36 Servo Servo4(D10);
Khanchana 0:3af6fae90816 37
Khanchana 0:3af6fae90816 38 void myservoLeft();
59010050 1:852156b5cca1 39 void move();
Khanchana 0:3af6fae90816 40
Khanchana 0:3af6fae90816 41 int pos_down = 1400;
Khanchana 0:3af6fae90816 42 int pos_up = 1000;
Khanchana 0:3af6fae90816 43 int pos_down_end = 1700;
59010050 1:852156b5cca1 44 int pos_up_end = ;
59010050 1:852156b5cca1 45 int final_posdown = 1700 ;
59010050 1:852156b5cca1 46 int final_posup = 1466.66;
59010050 1:852156b5cca1 47 int state_count = 1;
59010050 1:852156b5cca1 48 int round_count = 1;
Khanchana 0:3af6fae90816 49
Khanchana 0:3af6fae90816 50 int main() {
59010050 1:852156b5cca1 51 pc.baud(1000000);
59010050 1:852156b5cca1 52 pc.printf("malin");
59010050 1:852156b5cca1 53
59010050 1:852156b5cca1 54 timer1.start(); // start timer counting
59010050 1:852156b5cca1 55 if (pc.getc() == '1')
59010050 1:852156b5cca1 56 {
59010050 1:852156b5cca1 57 thread2.start(move);
59010050 1:852156b5cca1 58 thread1.start(IMU);
59010050 1:852156b5cca1 59 }
59010050 1:852156b5cca1 60
59010050 1:852156b5cca1 61 }
59010050 1:852156b5cca1 62 void move() {
59010050 1:852156b5cca1 63 attitude_setup();
Khanchana 0:3af6fae90816 64 Servo1.Enable(1000,2000);
Khanchana 0:3af6fae90816 65 Servo2.Enable(1000,2000);
Khanchana 0:3af6fae90816 66 Servo3.Enable(1000,2000);
Khanchana 0:3af6fae90816 67 Servo4.Enable(1000,2000);
59010050 1:852156b5cca1 68 pc.printf("start");
59010050 1:852156b5cca1 69 pc.printf("start");
59010050 1:852156b5cca1 70 pc.printf("start");
59010050 1:852156b5cca1 71 pc.printf("start");
59010050 1:852156b5cca1 72 pc.printf("start");
Khanchana 0:3af6fae90816 73 while(1){
59010050 1:852156b5cca1 74
59010050 1:852156b5cca1 75 if(state_count == 1){
Khanchana 0:3af6fae90816 76 Servo1.SetPosition(pos_down);
59010050 1:852156b5cca1 77 Servo3.SetPosition(pos_down);
59010050 1:852156b5cca1 78 pos_down = pos_down+5;
59010050 1:852156b5cca1 79 wait(0.01);
59010050 1:852156b5cca1 80 if(pos_down >= pos_down_end+5 and pos_up >= 1000){
59010050 1:852156b5cca1 81 state_count = 2;
59010050 1:852156b5cca1 82 }
Khanchana 0:3af6fae90816 83 }
59010050 1:852156b5cca1 84 else if(state_count == 2){
Khanchana 0:3af6fae90816 85 Servo2.SetPosition(pos_up);
59010050 1:852156b5cca1 86 Servo4.SetPosition(pos_up);
59010050 1:852156b5cca1 87 pos_up = pos_up+5;
59010050 1:852156b5cca1 88 wait(0.01);
59010050 1:852156b5cca1 89 if(pos_down >= pos_down_end+5 and pos_up >= pos_up_end+5){
59010050 1:852156b5cca1 90 state_count = 3;
59010050 1:852156b5cca1 91 }
Khanchana 0:3af6fae90816 92 }
59010050 1:852156b5cca1 93 else if(state_count == 3){
Khanchana 0:3af6fae90816 94 Servo1.SetPosition(pos_down);
59010050 1:852156b5cca1 95 Servo3.SetPosition(pos_down);
59010050 1:852156b5cca1 96 pos_down = pos_down-5;
59010050 1:852156b5cca1 97 wait(0.01);
59010050 1:852156b5cca1 98 if(pos_down <= 1395 and pos_up >= pos_up_end+5){
59010050 1:852156b5cca1 99 state_count = 4;
59010050 1:852156b5cca1 100 }
Khanchana 0:3af6fae90816 101 }
59010050 1:852156b5cca1 102 else if(state_count == 4){
Khanchana 0:3af6fae90816 103 Servo2.SetPosition(pos_up);
59010050 1:852156b5cca1 104 Servo4.SetPosition(pos_up);
59010050 1:852156b5cca1 105 pos_up = pos_up-5;
59010050 1:852156b5cca1 106 wait(0.01);
59010050 1:852156b5cca1 107 if(pos_down <= 1395 and pos_up <= 995){
59010050 1:852156b5cca1 108 state_count = 0;
59010050 1:852156b5cca1 109 }
59010050 1:852156b5cca1 110 }
59010050 1:852156b5cca1 111 else if (state_count == 0 and round_count < 10){
59010050 1:852156b5cca1 112 round_count = round_count+1;
59010050 1:852156b5cca1 113 state_count = 1;
59010050 1:852156b5cca1 114 pos_down = 1400;
59010050 1:852156b5cca1 115 pos_up = 1000;
59010050 1:852156b5cca1 116 }
59010050 1:852156b5cca1 117 else {
59010050 1:852156b5cca1 118 pc.printf("stop");
59010050 1:852156b5cca1 119 }
Khanchana 0:3af6fae90816 120 }
59010050 1:852156b5cca1 121 }