TurtleBot / Mbed 2 deprecated TurtleBot_V2

Changing about IMU

Dependencies:   Servo mbed-rtos mbed

Fork of TurtleBot_V1 by TurtleBot

main.cpp@3:5bfa7291c639, 2018-03-23 (annotated)

Committer:
worasuchad
Date:
Fri Mar 23 18:32:18 2018 +0000
Revision:
3:5bfa7291c639
Parent:
2:d4bd9ff10e8e 
edit and test IMU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
worasuchad 1:5609c1795245 1 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 2 // project: TurtleBot Project //
worasuchad 1:5609c1795245 3 // code v.: 1.0 //
worasuchad 1:5609c1795245 4 // board : NUCLEO-F303KB //
worasuchad 1:5609c1795245 5 // date : 20/2/2018 //
worasuchad 1:5609c1795245 6 // code by: Coding on Earth by Humans //
worasuchad 1:5609c1795245 7 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 8
worasuchad 1:5609c1795245 9 ///////////////////////// init ////////////////////////////////
worasuchad 1:5609c1795245 10 //////////////////////////////////////////////////////////////////
Khanchana 0:812929a5d5ad 11 #include "mbed.h"
Khanchana 0:812929a5d5ad 12 #include "rtos.h"
Khanchana 0:812929a5d5ad 13
Khanchana 0:812929a5d5ad 14 Serial pc(USBTX, USBRX);
worasuchad 1:5609c1795245 15
worasuchad 1:5609c1795245 16 Thread thread1; //control servo left
worasuchad 1:5609c1795245 17 Thread thread2; //control servo right
worasuchad 1:5609c1795245 18 Thread thread3; //read data from IMU
worasuchad 1:5609c1795245 19
worasuchad 1:5609c1795245 20 ///////////////////////// IMU ////////////////////////////////
worasuchad 1:5609c1795245 21 //////////////////////////////////////////////////////////////////
worasuchad 3:5bfa7291c639 22 #include "attitude.h"
worasuchad 3:5bfa7291c639 23 /*
worasuchad 1:5609c1795245 24 #include "MPU9250.h"
worasuchad 1:5609c1795245 25
worasuchad 1:5609c1795245 26 float sum = 0;
worasuchad 1:5609c1795245 27 uint32_t sumCount = 0;
worasuchad 1:5609c1795245 28 char buffer[14];
worasuchad 1:5609c1795245 29 float origin = 0;
worasuchad 1:5609c1795245 30
worasuchad 1:5609c1795245 31 MPU9250 mpu9250;
worasuchad 1:5609c1795245 32 Timer t;
worasuchad 3:5bfa7291c639 33 */
worasuchad 1:5609c1795245 34
worasuchad 1:5609c1795245 35 ///////////////////////// Servo ////////////////////////////////
worasuchad 1:5609c1795245 36 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 37 #include "Servo.h"
worasuchad 1:5609c1795245 38 Servo Servo1(D10);
Khanchana 0:812929a5d5ad 39 Servo Servo2(D6);
worasuchad 1:5609c1795245 40 Servo Servo3(D8);
worasuchad 1:5609c1795245 41 Servo Servo4(D9);
worasuchad 1:5609c1795245 42 /*
Khanchana 0:812929a5d5ad 43 int pos_up_start;
Khanchana 0:812929a5d5ad 44 int pos_up_end;
Khanchana 0:812929a5d5ad 45 int pos_down_start;
worasuchad 1:5609c1795245 46 int pos_down_end;*/
Khanchana 0:812929a5d5ad 47
worasuchad 1:5609c1795245 48 int pos_down_start = 1400;
worasuchad 1:5609c1795245 49 int pos_down_end = 1600;
worasuchad 1:5609c1795245 50 int pos_up_start = 1000;
worasuchad 1:5609c1795245 51 int pos_up_end = 1600;
Khanchana 0:812929a5d5ad 52
worasuchad 1:5609c1795245 53 ///////////////////////// prototype func ///////////////////////
worasuchad 1:5609c1795245 54 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 55 void myservoLeft();
worasuchad 1:5609c1795245 56 void myservoRight();
worasuchad 1:5609c1795245 57 void IMU();
Khanchana 0:812929a5d5ad 58
worasuchad 1:5609c1795245 59 ///////////////////////// main ////////////////////////////
worasuchad 1:5609c1795245 60 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 61 int main()
worasuchad 1:5609c1795245 62 {
worasuchad 3:5bfa7291c639 63
worasuchad 3:5bfa7291c639 64 IMU();
worasuchad 1:5609c1795245 65 thread1.start(myservoLeft);
worasuchad 1:5609c1795245 66 thread2.start(myservoRight);
worasuchad 1:5609c1795245 67 /* while(1)
worasuchad 1:5609c1795245 68 {
Khanchana 0:812929a5d5ad 69 printf("Hello World! Turtlebot is READY\n");
Khanchana 0:812929a5d5ad 70 printf("case 1-5\n");
worasuchad 1:5609c1795245 71 switch(pc.getc())
worasuchad 1:5609c1795245 72 {
Khanchana 0:812929a5d5ad 73 case '1':
Khanchana 0:812929a5d5ad 74 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 75 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 76 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 77 pos_up_end = 1700;
Khanchana 0:812929a5d5ad 78 break;
Khanchana 0:812929a5d5ad 79 case '2':
Khanchana 0:812929a5d5ad 80 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 81 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 82 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 83 pos_up_end = 1600;
Khanchana 0:812929a5d5ad 84 break;
Khanchana 0:812929a5d5ad 85 case '3':
Khanchana 0:812929a5d5ad 86 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 87 pos_down_end = 1650;
Khanchana 0:812929a5d5ad 88 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 89 pos_up_end = 1500;
Khanchana 0:812929a5d5ad 90 break;
Khanchana 0:812929a5d5ad 91 case '4':
Khanchana 0:812929a5d5ad 92 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 93 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 94 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 95 pos_up_end = 1650;
Khanchana 0:812929a5d5ad 96 break;
Khanchana 0:812929a5d5ad 97 case '5':
Khanchana 0:812929a5d5ad 98 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 99 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 100 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 101 pos_up_end = 1550;
Khanchana 0:812929a5d5ad 102 break;
Khanchana 0:812929a5d5ad 103 }
Khanchana 0:812929a5d5ad 104 printf("position down motor start = %d\n", pos_down_start);
Khanchana 0:812929a5d5ad 105 printf("position down motor end = %d\n", pos_down_end);
Khanchana 0:812929a5d5ad 106 printf("position up motor start = %d\n", pos_up_start);
Khanchana 0:812929a5d5ad 107 printf("position up motor end = %d\n", pos_up_end);
worasuchad 1:5609c1795245 108 thread1.start(myservoLeft);
worasuchad 1:5609c1795245 109 thread2.start(myservoRight);
worasuchad 1:5609c1795245 110 thread3.start(IMU);
worasuchad 1:5609c1795245 111 } */
worasuchad 1:5609c1795245 112 }
worasuchad 1:5609c1795245 113
worasuchad 1:5609c1795245 114
worasuchad 1:5609c1795245 115 ///////////////////////// myservoLeft /////////////////////////
worasuchad 1:5609c1795245 116 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 117 void myservoLeft()
worasuchad 1:5609c1795245 118 {
worasuchad 1:5609c1795245 119 for(int n = 0; n <= 5; n += 1)
worasuchad 1:5609c1795245 120 {
worasuchad 1:5609c1795245 121 Servo1.Enable(1000,20000);
worasuchad 1:5609c1795245 122 Servo2.Disable();
worasuchad 1:5609c1795245 123
worasuchad 1:5609c1795245 124 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5)
worasuchad 1:5609c1795245 125 {
worasuchad 1:5609c1795245 126 Servo1.SetPosition(pos);
worasuchad 1:5609c1795245 127 wait(0.01);
worasuchad 1:5609c1795245 128 }
worasuchad 1:5609c1795245 129
worasuchad 1:5609c1795245 130 Servo2.Enable(1000,20000);
worasuchad 1:5609c1795245 131 Servo1.Disable();
worasuchad 1:5609c1795245 132
worasuchad 1:5609c1795245 133 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5)
worasuchad 1:5609c1795245 134 {
worasuchad 1:5609c1795245 135 Servo2.SetPosition(pos);
worasuchad 1:5609c1795245 136 wait(0.01);
worasuchad 1:5609c1795245 137 }
worasuchad 1:5609c1795245 138
worasuchad 1:5609c1795245 139 Servo1.Enable(1000,20000);
worasuchad 1:5609c1795245 140 Servo2.Disable();
worasuchad 1:5609c1795245 141
worasuchad 1:5609c1795245 142 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5)
worasuchad 1:5609c1795245 143 {
worasuchad 1:5609c1795245 144 Servo1.SetPosition(pos);
worasuchad 1:5609c1795245 145 wait(0.01);
worasuchad 1:5609c1795245 146 }
worasuchad 1:5609c1795245 147
worasuchad 1:5609c1795245 148 Servo2.Enable(1000,20000);
worasuchad 1:5609c1795245 149 Servo1.Disable();
worasuchad 1:5609c1795245 150
worasuchad 1:5609c1795245 151 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5)
worasuchad 1:5609c1795245 152 {
worasuchad 1:5609c1795245 153 Servo2.SetPosition(pos);
worasuchad 1:5609c1795245 154 wait(0.01);
worasuchad 1:5609c1795245 155 }
worasuchad 1:5609c1795245 156 }
worasuchad 1:5609c1795245 157 }
worasuchad 1:5609c1795245 158
worasuchad 1:5609c1795245 159 ///////////////////////// myservoRight ///////////////////////
worasuchad 1:5609c1795245 160 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 161 void myservoRight()
worasuchad 1:5609c1795245 162 {
worasuchad 1:5609c1795245 163 for(int n = 0; n <= 5; n += 1)
worasuchad 1:5609c1795245 164 {
worasuchad 1:5609c1795245 165 Servo3.Enable(1000,20000);
worasuchad 1:5609c1795245 166 Servo4.Disable();
worasuchad 1:5609c1795245 167
worasuchad 1:5609c1795245 168 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5)
worasuchad 1:5609c1795245 169 {
worasuchad 1:5609c1795245 170 Servo3.SetPosition(pos);
worasuchad 1:5609c1795245 171 wait(0.01);
worasuchad 1:5609c1795245 172 }
worasuchad 1:5609c1795245 173
worasuchad 1:5609c1795245 174 Servo4.Enable(1000,20000);
worasuchad 1:5609c1795245 175 Servo3.Disable();
worasuchad 1:5609c1795245 176
worasuchad 1:5609c1795245 177 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5)
worasuchad 1:5609c1795245 178 {
worasuchad 1:5609c1795245 179 Servo4.SetPosition(pos);
worasuchad 1:5609c1795245 180 wait(0.01);
worasuchad 1:5609c1795245 181 }
worasuchad 1:5609c1795245 182
worasuchad 1:5609c1795245 183 Servo3.Enable(1000,20000);
worasuchad 1:5609c1795245 184 Servo4.Disable();
worasuchad 1:5609c1795245 185
worasuchad 1:5609c1795245 186 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5)
worasuchad 1:5609c1795245 187 {
worasuchad 1:5609c1795245 188 Servo3.SetPosition(pos);
worasuchad 1:5609c1795245 189 wait(0.01);
worasuchad 1:5609c1795245 190 }
worasuchad 1:5609c1795245 191
worasuchad 1:5609c1795245 192 Servo4.Enable(1000,20000);
worasuchad 1:5609c1795245 193 Servo3.Disable();
worasuchad 1:5609c1795245 194
worasuchad 1:5609c1795245 195 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5)
worasuchad 1:5609c1795245 196 {
worasuchad 1:5609c1795245 197 Servo4.SetPosition(pos);
worasuchad 1:5609c1795245 198 wait(0.01);
worasuchad 1:5609c1795245 199 }
Khanchana 0:812929a5d5ad 200 }
worasuchad 1:5609c1795245 201 }
worasuchad 1:5609c1795245 202
worasuchad 1:5609c1795245 203 ///////////////////////// IMU ///////////////////////
worasuchad 1:5609c1795245 204 //////////////////////////////////////////////////////////////////
worasuchad 3:5bfa7291c639 205
worasuchad 1:5609c1795245 206 void IMU()
worasuchad 1:5609c1795245 207 {
worasuchad 3:5bfa7291c639 208 attitude_setup();
worasuchad 3:5bfa7291c639 209 while(1)
worasuchad 1:5609c1795245 210 {
worasuchad 3:5bfa7291c639 211 attitude_get();
worasuchad 3:5bfa7291c639 212 pc.printf("%.0f %.0f %.0f \n\r", roll, pitch, yaw );
worasuchad 3:5bfa7291c639 213 }
worasuchad 3:5bfa7291c639 214 }