E2016-S1-Team5 / Mbed 2 deprecated Team5

Kildekode til robot

Dependencies:   PID mbed

Fork of Robotkode by Kim Nielsen

robot.cpp@2:1c27a43bb9b7, 2016-11-02 (annotated)

Committer:
kimnielsen
Date:
Wed Nov 02 08:05:41 2016 +0000
Revision:
2:1c27a43bb9b7
Parent:
1:396a582e8861
Child:
3:30bdc3d9e1c6
Team5 kildekode 2-11-16

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 2:1c27a43bb9b7 71 =============================================================================
kimnielsen 2:1c27a43bb9b7 72 Calculation of length between points
kimnielsen 2:1c27a43bb9b7 73 =============================================================================
kimnielsen 2:1c27a43bb9b7 74 */
kimnielsen 2:1c27a43bb9b7 75 double angle_desire = (second_y-first_y)/(second_x-first_x);
kimnielsen 2:1c27a43bb9b7 76 double error_present = atan(angle_desire)-0.785;
kimnielsen 2:1c27a43bb9b7 77
kimnielsen 0:d3dbe632b1a9 78 Timer t; // define a timer t
kimnielsen 0:d3dbe632b1a9 79 Serial pc(USBTX, USBRX); // not used here because we only have one serial
kimnielsen 0:d3dbe632b1a9 80 // connection
kimnielsen 0:d3dbe632b1a9 81 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
kimnielsen 0:d3dbe632b1a9 82 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
kimnielsen 0:d3dbe632b1a9 83
kimnielsen 2:1c27a43bb9b7 84 Wheel robot(PB_2, PB_1, PB_15, PB_14);
kimnielsen 2:1c27a43bb9b7 85 // create an object of robot H-bridge
kimnielsen 0:d3dbe632b1a9 86 // (M1A, M1B, M2A, M2B)
kimnielsen 0:d3dbe632b1a9 87
kimnielsen 2:1c27a43bb9b7 88 /*
kimnielsen 2:1c27a43bb9b7 89 =============================================================================
kimnielsen 2:1c27a43bb9b7 90 definition for analog power
kimnielsen 2:1c27a43bb9b7 91 =============================================================================
kimnielsen 0:d3dbe632b1a9 92 */
kimnielsen 0:d3dbe632b1a9 93 AnalogIn ain(PC_4);
kimnielsen 0:d3dbe632b1a9 94 DigitalOut dout(PB_13);
kimnielsen 0:d3dbe632b1a9 95 DigitalOut greenout(PB_12);
kimnielsen 0:d3dbe632b1a9 96
kimnielsen 0:d3dbe632b1a9 97 int stop=0;
kimnielsen 2:1c27a43bb9b7 98 /*
kimnielsen 2:1c27a43bb9b7 99 =============================================================================
kimnielsen 2:1c27a43bb9b7 100 Global variables
kimnielsen 2:1c27a43bb9b7 101 =============================================================================
kimnielsen 2:1c27a43bb9b7 102 */
kimnielsen 0:d3dbe632b1a9 103 double right,left;
kimnielsen 0:d3dbe632b1a9 104 double speed=0.5;
kimnielsen 0:d3dbe632b1a9 105 double e = 0; // angle error
kimnielsen 0:d3dbe632b1a9 106 int tickL = 0; // tick on left wheel
kimnielsen 0:d3dbe632b1a9 107 int tickR = 0; // tick on right wheel
kimnielsen 0:d3dbe632b1a9 108
kimnielsen 2:1c27a43bb9b7 109 /*
kimnielsen 2:1c27a43bb9b7 110 =============================================================================
kimnielsen 2:1c27a43bb9b7 111 Prototype defined functions
kimnielsen 2:1c27a43bb9b7 112 =============================================================================
kimnielsen 2:1c27a43bb9b7 113 */
kimnielsen 0:d3dbe632b1a9 114 void init();
kimnielsen 0:d3dbe632b1a9 115 void tickLeft(); // read tick left
kimnielsen 0:d3dbe632b1a9 116 void tickRight(); // read tick right
kimnielsen 0:d3dbe632b1a9 117 float Dleft(); // distance left wheel
kimnielsen 0:d3dbe632b1a9 118 float Dright(); // distance right wheel
kimnielsen 0:d3dbe632b1a9 119 float Dcenter(); // Distance for center
kimnielsen 2:1c27a43bb9b7 120
kimnielsen 2:1c27a43bb9b7 121 //functions for calculating desired distances from defined points
kimnielsen 2:1c27a43bb9b7 122 float Dist_reach1();
kimnielsen 2:1c27a43bb9b7 123 float Dist_reach2();
kimnielsen 2:1c27a43bb9b7 124 float Dist_reach3();
kimnielsen 2:1c27a43bb9b7 125
kimnielsen 0:d3dbe632b1a9 126 void get_to_goal ( ); // function to get to goal
kimnielsen 0:d3dbe632b1a9 127 void read_analog(); // comes here every second
kimnielsen 0:d3dbe632b1a9 128
kimnielsen 0:d3dbe632b1a9 129 Ticker T1; // create an object T1 of Ticker
kimnielsen 0:d3dbe632b1a9 130
kimnielsen 2:1c27a43bb9b7 131 /*
kimnielsen 2:1c27a43bb9b7 132 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 133 Accessing the int main
kimnielsen 2:1c27a43bb9b7 134 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 135 */
kimnielsen 0:d3dbe632b1a9 136 int main()
kimnielsen 0:d3dbe632b1a9 137 {
kimnielsen 0:d3dbe632b1a9 138 T1.attach(&read_analog, 1.0); // attach the address of the read_analog
kimnielsen 0:d3dbe632b1a9 139 //function to read analog in every second
kimnielsen 0:d3dbe632b1a9 140
kimnielsen 0:d3dbe632b1a9 141 tacho_left.rise(&tickLeft); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 142 //to the falling edge
kimnielsen 0:d3dbe632b1a9 143 tacho_right.rise(&tickRight); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 144 //to the falling edge
kimnielsen 0:d3dbe632b1a9 145
kimnielsen 0:d3dbe632b1a9 146 HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
kimnielsen 0:d3dbe632b1a9 147 // (echo,trigger) on pin PC_5, PC6
kimnielsen 0:d3dbe632b1a9 148 Servo servo1(PC_8); //Create an object of Servo class on pin PC_8
kimnielsen 0:d3dbe632b1a9 149
kimnielsen 0:d3dbe632b1a9 150 sensor.setRanges(2, 400); // set the range for Ultrasonic
kimnielsen 0:d3dbe632b1a9 151
kimnielsen 2:1c27a43bb9b7 152 /*
kimnielsen 2:1c27a43bb9b7 153 =============================================================================
kimnielsen 2:1c27a43bb9b7 154 Demo of the servor and ulta sonic sensor
kimnielsen 2:1c27a43bb9b7 155 =============================================================================
kimnielsen 2:1c27a43bb9b7 156 */
kimnielsen 0:d3dbe632b1a9 157 wait_ms(2000); // just get time for you to enable your screeen
kimnielsen 0:d3dbe632b1a9 158 servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
kimnielsen 0:d3dbe632b1a9 159 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 160 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
kimnielsen 0:d3dbe632b1a9 161 //readings from ultra sonic at this position
kimnielsen 0:d3dbe632b1a9 162 servo1.set_position(180); // Servo left position (angle = 180 degree)
kimnielsen 0:d3dbe632b1a9 163 //for servo
kimnielsen 0:d3dbe632b1a9 164 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 165 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 166 servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
kimnielsen 0:d3dbe632b1a9 167 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 168 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 169
kimnielsen 0:d3dbe632b1a9 170 init(); // initialise parameters (just for you to remember if you need to)
kimnielsen 0:d3dbe632b1a9 171
kimnielsen 0:d3dbe632b1a9 172 wait_ms(1000); //wait 1 secs here before you go
kimnielsen 0:d3dbe632b1a9 173 t.start(); // start timer (just demo of how you can use a timer)
kimnielsen 0:d3dbe632b1a9 174
kimnielsen 0:d3dbe632b1a9 175 /* ------------------
kimnielsen 0:d3dbe632b1a9 176 with PID
kimnielsen 0:d3dbe632b1a9 177 -----------------
kimnielsen 0:d3dbe632b1a9 178 -- delete get_to_goal(); when you use without pid
kimnielsen 0:d3dbe632b1a9 179 get_to_goal ();
kimnielsen 0:d3dbe632b1a9 180 */
kimnielsen 0:d3dbe632b1a9 181
kimnielsen 0:d3dbe632b1a9 182 /* ---------------------------
kimnielsen 0:d3dbe632b1a9 183 without pid
kimnielsen 0:d3dbe632b1a9 184 ------------------------
kimnielsen 0:d3dbe632b1a9 185 */
kimnielsen 0:d3dbe632b1a9 186 // delete between --- when you use it with pid
kimnielsen 0:d3dbe632b1a9 187 // ------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 188 /*
kimnielsen 2:1c27a43bb9b7 189 =============================================================================
kimnielsen 2:1c27a43bb9b7 190 Driving and stopping the robot with member functions from the wheel class
kimnielsen 2:1c27a43bb9b7 191 =============================================================================
kimnielsen 2:1c27a43bb9b7 192 */
kimnielsen 2:1c27a43bb9b7 193 while(Dcenter() <= Dist_reach1())
kimnielsen 2:1c27a43bb9b7 194 {
kimnielsen 2:1c27a43bb9b7 195 robot.FW(0.5, 0.5);
kimnielsen 2:1c27a43bb9b7 196 wait_ms(5000);
kimnielsen 0:d3dbe632b1a9 197 }
kimnielsen 0:d3dbe632b1a9 198 robot.stop(); // stop the robot again
kimnielsen 0:d3dbe632b1a9 199 printf("\n\rtimeused = %4d tickL= %4d tickR = %4d ", t.read_ms(),tickL,
kimnielsen 2:1c27a43bb9b7 200 tickR);
kimnielsen 2:1c27a43bb9b7 201
kimnielsen 2:1c27a43bb9b7 202 }
kimnielsen 2:1c27a43bb9b7 203 /*
kimnielsen 2:1c27a43bb9b7 204 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 205 END OF MAIN FUNCTION
kimnielsen 2:1c27a43bb9b7 206 ----------------------------------------------------------------------------
kimnielsen 2:1c27a43bb9b7 207 */
kimnielsen 0:d3dbe632b1a9 208
kimnielsen 2:1c27a43bb9b7 209 /*
kimnielsen 2:1c27a43bb9b7 210 =============================================================================
kimnielsen 2:1c27a43bb9b7 211 Calculations of distances traveled by both wheels and robot
kimnielsen 2:1c27a43bb9b7 212 =============================================================================
kimnielsen 2:1c27a43bb9b7 213 */
kimnielsen 0:d3dbe632b1a9 214 void tickLeft() // udtate left tick on tacho interrupt
kimnielsen 0:d3dbe632b1a9 215 {
kimnielsen 0:d3dbe632b1a9 216 tickL++;
kimnielsen 0:d3dbe632b1a9 217 }
kimnielsen 0:d3dbe632b1a9 218 void tickRight() // udtate right tick on tacho interrupt
kimnielsen 0:d3dbe632b1a9 219 {
kimnielsen 0:d3dbe632b1a9 220 tickR++;
kimnielsen 0:d3dbe632b1a9 221 }
kimnielsen 0:d3dbe632b1a9 222 float Dleft() // Distance for left wheel
kimnielsen 0:d3dbe632b1a9 223 {
kimnielsen 0:d3dbe632b1a9 224 return 2*PI*R*tickL/N;
kimnielsen 0:d3dbe632b1a9 225 }
kimnielsen 0:d3dbe632b1a9 226
kimnielsen 0:d3dbe632b1a9 227 float Dright() // Distance for right wheel
kimnielsen 0:d3dbe632b1a9 228 {
kimnielsen 0:d3dbe632b1a9 229 return 2*PI*R*tickR/N;
kimnielsen 0:d3dbe632b1a9 230 }
kimnielsen 0:d3dbe632b1a9 231
kimnielsen 0:d3dbe632b1a9 232 float Dcenter() // Distance for center
kimnielsen 0:d3dbe632b1a9 233 {
kimnielsen 0:d3dbe632b1a9 234 return (Dleft()+Dright())/2;
kimnielsen 0:d3dbe632b1a9 235 }
kimnielsen 0:d3dbe632b1a9 236
kimnielsen 2:1c27a43bb9b7 237 //Calculation of current position and new position with angles!!!
kimnielsen 0:d3dbe632b1a9 238
kimnielsen 2:1c27a43bb9b7 239 float angle_new()
kimnielsen 2:1c27a43bb9b7 240 {
kimnielsen 2:1c27a43bb9b7 241 return (Dright()-Dleft())/L;
kimnielsen 2:1c27a43bb9b7 242 }
kimnielsen 2:1c27a43bb9b7 243
kimnielsen 2:1c27a43bb9b7 244 float x_position()
kimnielsen 2:1c27a43bb9b7 245 {
kimnielsen 2:1c27a43bb9b7 246 return Dcenter()*sin(angle_new());
kimnielsen 2:1c27a43bb9b7 247 }
kimnielsen 2:1c27a43bb9b7 248
kimnielsen 2:1c27a43bb9b7 249 float y_position()
kimnielsen 2:1c27a43bb9b7 250 {
kimnielsen 2:1c27a43bb9b7 251 return Dcenter()*cos(angle_new());
kimnielsen 2:1c27a43bb9b7 252 }
kimnielsen 2:1c27a43bb9b7 253
kimnielsen 2:1c27a43bb9b7 254 //slet efterfølgende
kimnielsen 2:1c27a43bb9b7 255 float Dist_reach1()
kimnielsen 2:1c27a43bb9b7 256 {
kimnielsen 2:1c27a43bb9b7 257 double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
kimnielsen 2:1c27a43bb9b7 258 double dist_firstTosecond = sqrt(firstTosecond_calc);
kimnielsen 2:1c27a43bb9b7 259
kimnielsen 2:1c27a43bb9b7 260 return (dist_firstTosecond);
kimnielsen 2:1c27a43bb9b7 261 }
kimnielsen 2:1c27a43bb9b7 262
kimnielsen 2:1c27a43bb9b7 263 /*
kimnielsen 2:1c27a43bb9b7 264 =============================================================================
kimnielsen 2:1c27a43bb9b7 265 PID init
kimnielsen 2:1c27a43bb9b7 266 =============================================================================
kimnielsen 2:1c27a43bb9b7 267 */
kimnielsen 0:d3dbe632b1a9 268 void get_to_goal ( ) // function to get to goal
kimnielsen 0:d3dbe632b1a9 269 {
kimnielsen 2:1c27a43bb9b7 270 double power = 0;
kimnielsen 2:1c27a43bb9b7 271 double derivative = 0;
kimnielsen 2:1c27a43bb9b7 272 double proportional = 0;
kimnielsen 2:1c27a43bb9b7 273 double integral = 0;
kimnielsen 2:1c27a43bb9b7 274 double e_old = 0;
kimnielsen 0:d3dbe632b1a9 275
kimnielsen 2:1c27a43bb9b7 276 while (Dcenter() < Dist_reach1()) {
kimnielsen 2:1c27a43bb9b7 277 /*
kimnielsen 2:1c27a43bb9b7 278 -------------------------------------------------
kimnielsen 2:1c27a43bb9b7 279 error is difference between right tick and left tick
kimnielsen 2:1c27a43bb9b7 280 should be ajusted to your context, angle for robot
kimnielsen 2:1c27a43bb9b7 281 and instead of test Dcenter() in your while loop, test
kimnielsen 2:1c27a43bb9b7 282 for distance_to_goal()
kimnielsen 2:1c27a43bb9b7 283 -------------------------------------------------
kimnielsen 2:1c27a43bb9b7 284 */
kimnielsen 2:1c27a43bb9b7 285 e = tickR-tickL;
kimnielsen 2:1c27a43bb9b7 286 /*
kimnielsen 2:1c27a43bb9b7 287 =============================================================================
kimnielsen 2:1c27a43bb9b7 288 PID regulator
kimnielsen 2:1c27a43bb9b7 289 =============================================================================
kimnielsen 2:1c27a43bb9b7 290 */
kimnielsen 0:d3dbe632b1a9 291
kimnielsen 0:d3dbe632b1a9 292 // Compute the proportional
kimnielsen 0:d3dbe632b1a9 293 proportional = e; // get the error
kimnielsen 0:d3dbe632b1a9 294
kimnielsen 0:d3dbe632b1a9 295 // Compute the integral
kimnielsen 0:d3dbe632b1a9 296 integral += proportional;
kimnielsen 0:d3dbe632b1a9 297
kimnielsen 0:d3dbe632b1a9 298 // Compute the derivative
kimnielsen 0:d3dbe632b1a9 299 derivative = e - e_old;
kimnielsen 0:d3dbe632b1a9 300
kimnielsen 0:d3dbe632b1a9 301 // Remember the last error.
kimnielsen 0:d3dbe632b1a9 302 e_old = e;
kimnielsen 0:d3dbe632b1a9 303
kimnielsen 0:d3dbe632b1a9 304 // Compute the power
kimnielsen 0:d3dbe632b1a9 305 power = (proportional * (P_TERM)) + (integral * (I_TERM))
kimnielsen 0:d3dbe632b1a9 306 + (derivative * (D_TERM));
kimnielsen 0:d3dbe632b1a9 307
kimnielsen 0:d3dbe632b1a9 308 // Compute new speeds
kimnielsen 0:d3dbe632b1a9 309
kimnielsen 0:d3dbe632b1a9 310 right = speed - power;//if power is 0, no error and same speed on wheels
kimnielsen 0:d3dbe632b1a9 311 left = speed + power;
kimnielsen 0:d3dbe632b1a9 312
kimnielsen 0:d3dbe632b1a9 313
kimnielsen 0:d3dbe632b1a9 314 // limit checks
kimnielsen 0:d3dbe632b1a9 315 if (right < MIN)
kimnielsen 0:d3dbe632b1a9 316 right = MIN;
kimnielsen 0:d3dbe632b1a9 317 else if (right > MAX)
kimnielsen 0:d3dbe632b1a9 318 right = MAX;
kimnielsen 0:d3dbe632b1a9 319
kimnielsen 0:d3dbe632b1a9 320 if (left < MIN)
kimnielsen 0:d3dbe632b1a9 321 left = MIN;
kimnielsen 0:d3dbe632b1a9 322 else if (left > MAX)
kimnielsen 0:d3dbe632b1a9 323 left = MAX;
kimnielsen 0:d3dbe632b1a9 324
kimnielsen 0:d3dbe632b1a9 325 robot.FW(right,left); // set new positions
kimnielsen 0:d3dbe632b1a9 326 printf("\n\r Dcenter = %3.2f tickL= %4d tickR = %4d left %3.2f right %3.2f",
kimnielsen 0:d3dbe632b1a9 327 Dcenter(),tickL,tickR,left,right);
kimnielsen 0:d3dbe632b1a9 328
kimnielsen 2:1c27a43bb9b7 329 wait_ms(20); // should be adjusted to your context. Give the motor time
kimnielsen 0:d3dbe632b1a9 330 // to do something before you react
kimnielsen 0:d3dbe632b1a9 331 }
kimnielsen 0:d3dbe632b1a9 332 t.stop(); // stop timer
kimnielsen 0:d3dbe632b1a9 333 }
kimnielsen 0:d3dbe632b1a9 334
kimnielsen 0:d3dbe632b1a9 335 void read_analog() // comes here every second in this case
kimnielsen 0:d3dbe632b1a9 336 // could be flagged with global variables like "stop"
kimnielsen 0:d3dbe632b1a9 337 {
kimnielsen 0:d3dbe632b1a9 338 if(ain > 0.6f) { // 60% power, time for recharge
kimnielsen 0:d3dbe632b1a9 339 dout = 1;
kimnielsen 0:d3dbe632b1a9 340 stop=0;
kimnielsen 0:d3dbe632b1a9 341 } else {
kimnielsen 0:d3dbe632b1a9 342 dout = not dout;
kimnielsen 0:d3dbe632b1a9 343 stop=1; // flash red led
kimnielsen 0:d3dbe632b1a9 344 }
kimnielsen 0:d3dbe632b1a9 345
kimnielsen 0:d3dbe632b1a9 346 if (ain==1.0f) greenout = 1; // full power
kimnielsen 0:d3dbe632b1a9 347 else greenout = 0;
kimnielsen 0:d3dbe632b1a9 348 }
kimnielsen 0:d3dbe632b1a9 349 void init() // init your parameters
kimnielsen 0:d3dbe632b1a9 350 {
kimnielsen 0:d3dbe632b1a9 351 tickL=0;
kimnielsen 0:d3dbe632b1a9 352 tickR=0;
kimnielsen 0:d3dbe632b1a9 353 }