Kim Nielsen / Mbed 2 deprecated Team5

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Team5_kode by E2016-S1-Team5

robot.cpp@3:30bdc3d9e1c6, 2016-11-02 (annotated)

Committer:
kimnielsen
Date:
Wed Nov 02 08:11:24 2016 +0000
Revision:
3:30bdc3d9e1c6
Parent:
2:1c27a43bb9b7
Child:
4:62a6681510d6
asd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kimnielsen 0:d3dbe632b1a9 1 /*
kimnielsen 0:d3dbe632b1a9 2 ============================================================================
kimnielsen 0:d3dbe632b1a9 3 Name : robot.cpp
kimnielsen 2:1c27a43bb9b7 4 Author : Team 5
kimnielsen 0:d3dbe632b1a9 5 Version : 0.1
kimnielsen 2:1c27a43bb9b7 6 Date : 13-10-2016
kimnielsen 0:d3dbe632b1a9 7 Copyright : Open for all
kimnielsen 0:d3dbe632b1a9 8 Description : Program to serve af platform for Pro1 2016
kimnielsen 0:d3dbe632b1a9 9 ============================================================================
kimnielsen 0:d3dbe632b1a9 10 */
kimnielsen 2:1c27a43bb9b7 11
kimnielsen 2:1c27a43bb9b7 12 /*
kimnielsen 2:1c27a43bb9b7 13 =============================================================================
kimnielsen 2:1c27a43bb9b7 14 Including the necessary header files
kimnielsen 2:1c27a43bb9b7 15 =============================================================================
kimnielsen 2:1c27a43bb9b7 16 */
kimnielsen 0:d3dbe632b1a9 17 #include "mbed.h"
kimnielsen 0:d3dbe632b1a9 18 #include "HCSR04.h"
kimnielsen 0:d3dbe632b1a9 19 #include "nucleo_servo.h"
kimnielsen 0:d3dbe632b1a9 20 #include "hack_motor.h"
kimnielsen 2:1c27a43bb9b7 21 #include <math.h>
kimnielsen 0:d3dbe632b1a9 22
kimnielsen 2:1c27a43bb9b7 23 /*
kimnielsen 2:1c27a43bb9b7 24 =============================================================================
kimnielsen 2:1c27a43bb9b7 25 PID definitions
kimnielsen 2:1c27a43bb9b7 26 =============================================================================
kimnielsen 2:1c27a43bb9b7 27 */
kimnielsen 2:1c27a43bb9b7 28 #define P_TERM 0.2
kimnielsen 2:1c27a43bb9b7 29 #define I_TERM 0.1
kimnielsen 2:1c27a43bb9b7 30 #define D_TERM 0.3
kimnielsen 2:1c27a43bb9b7 31 #define MAX 1.0
kimnielsen 0:d3dbe632b1a9 32 #define MIN 0
kimnielsen 2:1c27a43bb9b7 33 #define PI 3.141592
kimnielsen 0:d3dbe632b1a9 34
kimnielsen 2:1c27a43bb9b7 35 /*
kimnielsen 2:1c27a43bb9b7 36 =============================================================================
kimnielsen 2:1c27a43bb9b7 37 Geometry for robot:
kimnielsen 2:1c27a43bb9b7 38 note that N,R,L maybe not have the most meaningful name
kimnielsen 2:1c27a43bb9b7 39 but they follow the names from the theory for our
kimnielsen 2:1c27a43bb9b7 40 robot lectures, and can be used in youa application
kimnielsen 2:1c27a43bb9b7 41 =============================================================================
kimnielsen 2:1c27a43bb9b7 42 */
kimnielsen 0:d3dbe632b1a9 43 #define N 20 // ticks on wheel
kimnielsen 0:d3dbe632b1a9 44 #define R 32.5 // radius = 32.5 mm
kimnielsen 0:d3dbe632b1a9 45 #define L 133 // 133 mm distance between wheels
kimnielsen 2:1c27a43bb9b7 46
kimnielsen 2:1c27a43bb9b7 47 /*
kimnielsen 2:1c27a43bb9b7 48 =============================================================================
kimnielsen 2:1c27a43bb9b7 49 Coordinates for robot to reach
kimnielsen 2:1c27a43bb9b7 50 =============================================================================
kimnielsen 2:1c27a43bb9b7 51 */
kimnielsen 2:1c27a43bb9b7 52 //Start position:
kimnielsen 2:1c27a43bb9b7 53 #define first_x 0
kimnielsen 2:1c27a43bb9b7 54 #define first_y 0
kimnielsen 2:1c27a43bb9b7 55 #define first_angle 0.785 //45 deg
kimnielsen 0:d3dbe632b1a9 56
kimnielsen 2:1c27a43bb9b7 57 //Second position:
kimnielsen 2:1c27a43bb9b7 58 #define second_x 50
kimnielsen 2:1c27a43bb9b7 59 #define second_y 40
kimnielsen 2:1c27a43bb9b7 60 #define second_angle 1.047 //60 deg
kimnielsen 2:1c27a43bb9b7 61
kimnielsen 2:1c27a43bb9b7 62 //Third position:
kimnielsen 2:1c27a43bb9b7 63 #define third_x 70
kimnielsen 2:1c27a43bb9b7 64 #define third_y 10
kimnielsen 2:1c27a43bb9b7 65 #define third_angle 1.396 //80 deg
kimnielsen 0:d3dbe632b1a9 66
kimnielsen 2:1c27a43bb9b7 67 //fourth position:
kimnielsen 2:1c27a43bb9b7 68 #define fourth_x 70
kimnielsen 2:1c27a43bb9b7 69 #define fourth_y 50
kimnielsen 2:1c27a43bb9b7 70
kimnielsen 0:d3dbe632b1a9 71 Timer t; // define a timer t
kimnielsen 0:d3dbe632b1a9 72 Serial pc(USBTX, USBRX); // not used here because we only have one serial
kimnielsen 0:d3dbe632b1a9 73 // connection
kimnielsen 0:d3dbe632b1a9 74 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
kimnielsen 0:d3dbe632b1a9 75 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
kimnielsen 0:d3dbe632b1a9 76
kimnielsen 2:1c27a43bb9b7 77 Wheel robot(PB_2, PB_1, PB_15, PB_14);
kimnielsen 2:1c27a43bb9b7 78 // create an object of robot H-bridge
kimnielsen 0:d3dbe632b1a9 79 // (M1A, M1B, M2A, M2B)
kimnielsen 0:d3dbe632b1a9 80
kimnielsen 2:1c27a43bb9b7 81 /*
kimnielsen 2:1c27a43bb9b7 82 =============================================================================
kimnielsen 2:1c27a43bb9b7 83 definition for analog power
kimnielsen 2:1c27a43bb9b7 84 =============================================================================
kimnielsen 0:d3dbe632b1a9 85 */
kimnielsen 0:d3dbe632b1a9 86 AnalogIn ain(PC_4);
kimnielsen 0:d3dbe632b1a9 87 DigitalOut dout(PB_13);
kimnielsen 0:d3dbe632b1a9 88 DigitalOut greenout(PB_12);
kimnielsen 0:d3dbe632b1a9 89
kimnielsen 0:d3dbe632b1a9 90 int stop=0;
kimnielsen 2:1c27a43bb9b7 91 /*
kimnielsen 2:1c27a43bb9b7 92 =============================================================================
kimnielsen 2:1c27a43bb9b7 93 Global variables
kimnielsen 2:1c27a43bb9b7 94 =============================================================================
kimnielsen 2:1c27a43bb9b7 95 */
kimnielsen 0:d3dbe632b1a9 96 double right,left;
kimnielsen 0:d3dbe632b1a9 97 double speed=0.5;
kimnielsen 0:d3dbe632b1a9 98 double e = 0; // angle error
kimnielsen 0:d3dbe632b1a9 99 int tickL = 0; // tick on left wheel
kimnielsen 0:d3dbe632b1a9 100 int tickR = 0; // tick on right wheel
kimnielsen 0:d3dbe632b1a9 101
kimnielsen 2:1c27a43bb9b7 102 /*
kimnielsen 2:1c27a43bb9b7 103 =============================================================================
kimnielsen 2:1c27a43bb9b7 104 Prototype defined functions
kimnielsen 2:1c27a43bb9b7 105 =============================================================================
kimnielsen 2:1c27a43bb9b7 106 */
kimnielsen 0:d3dbe632b1a9 107 void init();
kimnielsen 0:d3dbe632b1a9 108 void tickLeft(); // read tick left
kimnielsen 0:d3dbe632b1a9 109 void tickRight(); // read tick right
kimnielsen 0:d3dbe632b1a9 110 float Dleft(); // distance left wheel
kimnielsen 0:d3dbe632b1a9 111 float Dright(); // distance right wheel
kimnielsen 0:d3dbe632b1a9 112 float Dcenter(); // Distance for center
kimnielsen 2:1c27a43bb9b7 113
kimnielsen 2:1c27a43bb9b7 114 //functions for calculating desired distances from defined points
kimnielsen 2:1c27a43bb9b7 115 float Dist_reach1();
kimnielsen 2:1c27a43bb9b7 116 float Dist_reach2();
kimnielsen 2:1c27a43bb9b7 117 float Dist_reach3();
kimnielsen 2:1c27a43bb9b7 118
kimnielsen 0:d3dbe632b1a9 119 void get_to_goal ( ); // function to get to goal
kimnielsen 0:d3dbe632b1a9 120 void read_analog(); // comes here every second
kimnielsen 0:d3dbe632b1a9 121
kimnielsen 0:d3dbe632b1a9 122 Ticker T1; // create an object T1 of Ticker
kimnielsen 0:d3dbe632b1a9 123
kimnielsen 2:1c27a43bb9b7 124 /*
kimnielsen 2:1c27a43bb9b7 125 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 126 Accessing the int main
kimnielsen 2:1c27a43bb9b7 127 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 128 */
kimnielsen 0:d3dbe632b1a9 129 int main()
kimnielsen 0:d3dbe632b1a9 130 {
kimnielsen 0:d3dbe632b1a9 131 T1.attach(&read_analog, 1.0); // attach the address of the read_analog
kimnielsen 0:d3dbe632b1a9 132 //function to read analog in every second
kimnielsen 0:d3dbe632b1a9 133
kimnielsen 0:d3dbe632b1a9 134 tacho_left.rise(&tickLeft); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 135 //to the falling edge
kimnielsen 0:d3dbe632b1a9 136 tacho_right.rise(&tickRight); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 137 //to the falling edge
kimnielsen 0:d3dbe632b1a9 138
kimnielsen 0:d3dbe632b1a9 139 HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
kimnielsen 0:d3dbe632b1a9 140 // (echo,trigger) on pin PC_5, PC6
kimnielsen 0:d3dbe632b1a9 141 Servo servo1(PC_8); //Create an object of Servo class on pin PC_8
kimnielsen 0:d3dbe632b1a9 142
kimnielsen 0:d3dbe632b1a9 143 sensor.setRanges(2, 400); // set the range for Ultrasonic
kimnielsen 0:d3dbe632b1a9 144
kimnielsen 2:1c27a43bb9b7 145 /*
kimnielsen 2:1c27a43bb9b7 146 =============================================================================
kimnielsen 2:1c27a43bb9b7 147 Demo of the servor and ulta sonic sensor
kimnielsen 2:1c27a43bb9b7 148 =============================================================================
kimnielsen 2:1c27a43bb9b7 149 */
kimnielsen 0:d3dbe632b1a9 150 wait_ms(2000); // just get time for you to enable your screeen
kimnielsen 0:d3dbe632b1a9 151 servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
kimnielsen 0:d3dbe632b1a9 152 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 153 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
kimnielsen 0:d3dbe632b1a9 154 //readings from ultra sonic at this position
kimnielsen 0:d3dbe632b1a9 155 servo1.set_position(180); // Servo left position (angle = 180 degree)
kimnielsen 0:d3dbe632b1a9 156 //for servo
kimnielsen 0:d3dbe632b1a9 157 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 158 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 159 servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
kimnielsen 0:d3dbe632b1a9 160 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 161 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 162
kimnielsen 0:d3dbe632b1a9 163 init(); // initialise parameters (just for you to remember if you need to)
kimnielsen 0:d3dbe632b1a9 164
kimnielsen 0:d3dbe632b1a9 165 wait_ms(1000); //wait 1 secs here before you go
kimnielsen 0:d3dbe632b1a9 166 t.start(); // start timer (just demo of how you can use a timer)
kimnielsen 0:d3dbe632b1a9 167
kimnielsen 0:d3dbe632b1a9 168 /* ------------------
kimnielsen 0:d3dbe632b1a9 169 with PID
kimnielsen 0:d3dbe632b1a9 170 -----------------
kimnielsen 0:d3dbe632b1a9 171 -- delete get_to_goal(); when you use without pid
kimnielsen 0:d3dbe632b1a9 172 get_to_goal ();
kimnielsen 0:d3dbe632b1a9 173 */
kimnielsen 0:d3dbe632b1a9 174
kimnielsen 0:d3dbe632b1a9 175 /* ---------------------------
kimnielsen 0:d3dbe632b1a9 176 without pid
kimnielsen 0:d3dbe632b1a9 177 ------------------------
kimnielsen 0:d3dbe632b1a9 178 */
kimnielsen 0:d3dbe632b1a9 179 // delete between --- when you use it with pid
kimnielsen 0:d3dbe632b1a9 180 // ------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 181 /*
kimnielsen 2:1c27a43bb9b7 182 =============================================================================
kimnielsen 2:1c27a43bb9b7 183 Driving and stopping the robot with member functions from the wheel class
kimnielsen 2:1c27a43bb9b7 184 =============================================================================
kimnielsen 2:1c27a43bb9b7 185 */
kimnielsen 2:1c27a43bb9b7 186 while(Dcenter() <= Dist_reach1())
kimnielsen 2:1c27a43bb9b7 187 {
kimnielsen 2:1c27a43bb9b7 188 robot.FW(0.5, 0.5);
kimnielsen 2:1c27a43bb9b7 189 wait_ms(5000);
kimnielsen 0:d3dbe632b1a9 190 }
kimnielsen 0:d3dbe632b1a9 191 robot.stop(); // stop the robot again
kimnielsen 0:d3dbe632b1a9 192 printf("\n\rtimeused = %4d tickL= %4d tickR = %4d ", t.read_ms(),tickL,
kimnielsen 2:1c27a43bb9b7 193 tickR);
kimnielsen 2:1c27a43bb9b7 194
kimnielsen 2:1c27a43bb9b7 195 }
kimnielsen 2:1c27a43bb9b7 196 /*
kimnielsen 2:1c27a43bb9b7 197 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 198 END OF MAIN FUNCTION
kimnielsen 2:1c27a43bb9b7 199 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 200 */
kimnielsen 0:d3dbe632b1a9 201
kimnielsen 2:1c27a43bb9b7 202 /*
kimnielsen 2:1c27a43bb9b7 203 =============================================================================
kimnielsen 2:1c27a43bb9b7 204 Calculations of distances traveled by both wheels and robot
kimnielsen 2:1c27a43bb9b7 205 =============================================================================
kimnielsen 2:1c27a43bb9b7 206 */
kimnielsen 0:d3dbe632b1a9 207 void tickLeft() // udtate left tick on tacho interrupt
kimnielsen 0:d3dbe632b1a9 208 {
kimnielsen 0:d3dbe632b1a9 209 tickL++;
kimnielsen 0:d3dbe632b1a9 210 }
kimnielsen 0:d3dbe632b1a9 211 void tickRight() // udtate right tick on tacho interrupt
kimnielsen 0:d3dbe632b1a9 212 {
kimnielsen 0:d3dbe632b1a9 213 tickR++;
kimnielsen 0:d3dbe632b1a9 214 }
kimnielsen 0:d3dbe632b1a9 215 float Dleft() // Distance for left wheel
kimnielsen 0:d3dbe632b1a9 216 {
kimnielsen 0:d3dbe632b1a9 217 return 2*PI*R*tickL/N;
kimnielsen 0:d3dbe632b1a9 218 }
kimnielsen 0:d3dbe632b1a9 219
kimnielsen 0:d3dbe632b1a9 220 float Dright() // Distance for right wheel
kimnielsen 0:d3dbe632b1a9 221 {
kimnielsen 0:d3dbe632b1a9 222 return 2*PI*R*tickR/N;
kimnielsen 0:d3dbe632b1a9 223 }
kimnielsen 0:d3dbe632b1a9 224
kimnielsen 0:d3dbe632b1a9 225 float Dcenter() // Distance for center
kimnielsen 0:d3dbe632b1a9 226 {
kimnielsen 0:d3dbe632b1a9 227 return (Dleft()+Dright())/2;
kimnielsen 0:d3dbe632b1a9 228 }
kimnielsen 0:d3dbe632b1a9 229
kimnielsen 2:1c27a43bb9b7 230 //Calculation of current position and new position with angles!!!
kimnielsen 0:d3dbe632b1a9 231
kimnielsen 2:1c27a43bb9b7 232 float angle_new()
kimnielsen 2:1c27a43bb9b7 233 {
kimnielsen 2:1c27a43bb9b7 234 return (Dright()-Dleft())/L;
kimnielsen 2:1c27a43bb9b7 235 }
kimnielsen 2:1c27a43bb9b7 236
kimnielsen 2:1c27a43bb9b7 237 float x_position()
kimnielsen 2:1c27a43bb9b7 238 {
kimnielsen 2:1c27a43bb9b7 239 return Dcenter()*sin(angle_new());
kimnielsen 2:1c27a43bb9b7 240 }
kimnielsen 2:1c27a43bb9b7 241
kimnielsen 2:1c27a43bb9b7 242 float y_position()
kimnielsen 2:1c27a43bb9b7 243 {
kimnielsen 2:1c27a43bb9b7 244 return Dcenter()*cos(angle_new());
kimnielsen 2:1c27a43bb9b7 245 }
kimnielsen 2:1c27a43bb9b7 246
kimnielsen 2:1c27a43bb9b7 247 //slet efterfølgende
kimnielsen 2:1c27a43bb9b7 248 float Dist_reach1()
kimnielsen 2:1c27a43bb9b7 249 {
kimnielsen 2:1c27a43bb9b7 250 double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
kimnielsen 2:1c27a43bb9b7 251 double dist_firstTosecond = sqrt(firstTosecond_calc);
kimnielsen 2:1c27a43bb9b7 252
kimnielsen 2:1c27a43bb9b7 253 return (dist_firstTosecond);
kimnielsen 2:1c27a43bb9b7 254 }
kimnielsen 2:1c27a43bb9b7 255
kimnielsen 2:1c27a43bb9b7 256 /*
kimnielsen 2:1c27a43bb9b7 257 =============================================================================
kimnielsen 2:1c27a43bb9b7 258 PID init
kimnielsen 2:1c27a43bb9b7 259 =============================================================================
kimnielsen 2:1c27a43bb9b7 260 */
kimnielsen 0:d3dbe632b1a9 261 void get_to_goal ( ) // function to get to goal
kimnielsen 0:d3dbe632b1a9 262 {
kimnielsen 2:1c27a43bb9b7 263 double power = 0;
kimnielsen 2:1c27a43bb9b7 264 double derivative = 0;
kimnielsen 2:1c27a43bb9b7 265 double proportional = 0;
kimnielsen 2:1c27a43bb9b7 266 double integral = 0;
kimnielsen 2:1c27a43bb9b7 267 double e_old = 0;
kimnielsen 0:d3dbe632b1a9 268
kimnielsen 2:1c27a43bb9b7 269 while (Dcenter() < Dist_reach1()) {
kimnielsen 2:1c27a43bb9b7 270 /*
kimnielsen 2:1c27a43bb9b7 271 -------------------------------------------------
kimnielsen 2:1c27a43bb9b7 272 error is difference between right tick and left tick
kimnielsen 2:1c27a43bb9b7 273 should be ajusted to your context, angle for robot
kimnielsen 2:1c27a43bb9b7 274 and instead of test Dcenter() in your while loop, test
kimnielsen 2:1c27a43bb9b7 275 for distance_to_goal()
kimnielsen 2:1c27a43bb9b7 276 -------------------------------------------------
kimnielsen 2:1c27a43bb9b7 277 */
kimnielsen 2:1c27a43bb9b7 278 e = tickR-tickL;
kimnielsen 2:1c27a43bb9b7 279 /*
kimnielsen 2:1c27a43bb9b7 280 =============================================================================
kimnielsen 2:1c27a43bb9b7 281 PID regulator
kimnielsen 2:1c27a43bb9b7 282 =============================================================================
kimnielsen 2:1c27a43bb9b7 283 */
kimnielsen 0:d3dbe632b1a9 284
kimnielsen 0:d3dbe632b1a9 285 // Compute the proportional
kimnielsen 0:d3dbe632b1a9 286 proportional = e; // get the error
kimnielsen 0:d3dbe632b1a9 287
kimnielsen 0:d3dbe632b1a9 288 // Compute the integral
kimnielsen 0:d3dbe632b1a9 289 integral += proportional;
kimnielsen 0:d3dbe632b1a9 290
kimnielsen 0:d3dbe632b1a9 291 // Compute the derivative
kimnielsen 0:d3dbe632b1a9 292 derivative = e - e_old;
kimnielsen 0:d3dbe632b1a9 293
kimnielsen 0:d3dbe632b1a9 294 // Remember the last error.
kimnielsen 0:d3dbe632b1a9 295 e_old = e;
kimnielsen 0:d3dbe632b1a9 296
kimnielsen 0:d3dbe632b1a9 297 // Compute the power
kimnielsen 0:d3dbe632b1a9 298 power = (proportional * (P_TERM)) + (integral * (I_TERM))
kimnielsen 0:d3dbe632b1a9 299 + (derivative * (D_TERM));
kimnielsen 0:d3dbe632b1a9 300
kimnielsen 0:d3dbe632b1a9 301 // Compute new speeds
kimnielsen 0:d3dbe632b1a9 302
kimnielsen 0:d3dbe632b1a9 303 right = speed - power;//if power is 0, no error and same speed on wheels
kimnielsen 0:d3dbe632b1a9 304 left = speed + power;
kimnielsen 0:d3dbe632b1a9 305
kimnielsen 0:d3dbe632b1a9 306
kimnielsen 0:d3dbe632b1a9 307 // limit checks
kimnielsen 0:d3dbe632b1a9 308 if (right < MIN)
kimnielsen 0:d3dbe632b1a9 309 right = MIN;
kimnielsen 0:d3dbe632b1a9 310 else if (right > MAX)
kimnielsen 0:d3dbe632b1a9 311 right = MAX;
kimnielsen 0:d3dbe632b1a9 312
kimnielsen 0:d3dbe632b1a9 313 if (left < MIN)
kimnielsen 0:d3dbe632b1a9 314 left = MIN;
kimnielsen 0:d3dbe632b1a9 315 else if (left > MAX)
kimnielsen 0:d3dbe632b1a9 316 left = MAX;
kimnielsen 0:d3dbe632b1a9 317
kimnielsen 0:d3dbe632b1a9 318 robot.FW(right,left); // set new positions
kimnielsen 0:d3dbe632b1a9 319 printf("\n\r Dcenter = %3.2f tickL= %4d tickR = %4d left %3.2f right %3.2f",
kimnielsen 0:d3dbe632b1a9 320 Dcenter(),tickL,tickR,left,right);
kimnielsen 0:d3dbe632b1a9 321
kimnielsen 2:1c27a43bb9b7 322 wait_ms(20); // should be adjusted to your context. Give the motor time
kimnielsen 0:d3dbe632b1a9 323 // to do something before you react
kimnielsen 0:d3dbe632b1a9 324 }
kimnielsen 0:d3dbe632b1a9 325 t.stop(); // stop timer
kimnielsen 0:d3dbe632b1a9 326 }
kimnielsen 0:d3dbe632b1a9 327
kimnielsen 0:d3dbe632b1a9 328 void read_analog() // comes here every second in this case
kimnielsen 0:d3dbe632b1a9 329 // could be flagged with global variables like "stop"
kimnielsen 0:d3dbe632b1a9 330 {
kimnielsen 0:d3dbe632b1a9 331 if(ain > 0.6f) { // 60% power, time for recharge
kimnielsen 0:d3dbe632b1a9 332 dout = 1;
kimnielsen 0:d3dbe632b1a9 333 stop=0;
kimnielsen 0:d3dbe632b1a9 334 } else {
kimnielsen 0:d3dbe632b1a9 335 dout = not dout;
kimnielsen 0:d3dbe632b1a9 336 stop=1; // flash red led
kimnielsen 0:d3dbe632b1a9 337 }
kimnielsen 0:d3dbe632b1a9 338
kimnielsen 0:d3dbe632b1a9 339 if (ain==1.0f) greenout = 1; // full power
kimnielsen 0:d3dbe632b1a9 340 else greenout = 0;
kimnielsen 0:d3dbe632b1a9 341 }
kimnielsen 0:d3dbe632b1a9 342 void init() // init your parameters
kimnielsen 0:d3dbe632b1a9 343 {
kimnielsen 0:d3dbe632b1a9 344 tickL=0;
kimnielsen 0:d3dbe632b1a9 345 tickR=0;
kimnielsen 0:d3dbe632b1a9 346 }