Alexandre Oliveira / Mbed 2 deprecated robot_eraser

Magnetic Whiteboard Robot Eraser

Dependencies:   Motordriver mbed

Fork of LSM9DS0 by Taylor Andrews

main.cpp@6:a37a49d2793c, 2015-05-01 (annotated)

Committer:
alexcrepory
Date:
Fri May 01 04:59:07 2015 +0000
Revision:
6:a37a49d2793c
Parent:
5:e6a15dcba942 
1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
randrews33 0:1b975a6ae539 1 #include "LSM9DS0.h"
randrews33 0:1b975a6ae539 2 #include "mbed.h"
alexcrepory 6:a37a49d2793c 3 #include "motordriver.h"
alexcrepory 6:a37a49d2793c 4
alexcrepory 6:a37a49d2793c 5 #define pi 3.14159265359
randrews33 0:1b975a6ae539 6
randrews33 0:1b975a6ae539 7 // SDO_XM and SDO_G are both grounded, so our addresses are:
randrews33 4:bf8f4e7c9905 8 #define LSM9DS0_XM 0x1D // Would be 0x1E if SDO_XM is LOW
randrews33 4:bf8f4e7c9905 9 #define LSM9DS0_G 0x6B // Would be 0x6A if SDO_G is LOW
randrews33 0:1b975a6ae539 10
alexcrepory 6:a37a49d2793c 11 Ticker tmr;
randrews33 4:bf8f4e7c9905 12 LSM9DS0 dof(p9, p10,LSM9DS0_G, LSM9DS0_XM);
randrews33 0:1b975a6ae539 13 Serial pc(USBTX, USBRX); // tx, rx
alexcrepory 6:a37a49d2793c 14 Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
alexcrepory 6:a37a49d2793c 15 Motor right(p26, p25, p24, 1);
alexcrepory 6:a37a49d2793c 16 InterruptIn button(p7);
randrews33 0:1b975a6ae539 17
alexcrepory 6:a37a49d2793c 18 DigitalOut timer_on(LED1);
alexcrepory 6:a37a49d2793c 19 DigitalOut pb(LED2);
alexcrepory 6:a37a49d2793c 20 DigitalOut ld3(LED3);
alexcrepory 6:a37a49d2793c 21 DigitalOut ld4(LED4);
randrews33 0:1b975a6ae539 22
alexcrepory 6:a37a49d2793c 23 #define R 0.06 //distance from the center of the robot to the wheel (radius)
alexcrepory 6:a37a49d2793c 24 #define lambda_r 22 // eigenvalue fo the regulator, chosen through tests
alexcrepory 6:a37a49d2793c 25
alexcrepory 6:a37a49d2793c 26 float umax=1; //maximum system input (motor speed)
randrews33 4:bf8f4e7c9905 27
alexcrepory 6:a37a49d2793c 28 float K=lambda_r*R; //control gain
alexcrepory 6:a37a49d2793c 29 float T=0.01; //in milliseconds
randrews33 0:1b975a6ae539 30
alexcrepory 6:a37a49d2793c 31 float w=0; //angular velocity
alexcrepory 6:a37a49d2793c 32 float r=0; //reference for orientation
alexcrepory 6:a37a49d2793c 33 float y=0; //current orientation
alexcrepory 6:a37a49d2793c 34 float u=0; //difference between right and left motor speeds
alexcrepory 6:a37a49d2793c 35 float u_star=0;
alexcrepory 6:a37a49d2793c 36
alexcrepory 6:a37a49d2793c 37 float vr=umax/2; //right motor speed
alexcrepory 6:a37a49d2793c 38 float vl=umax/2; //left motor speed
alexcrepory 6:a37a49d2793c 39
alexcrepory 6:a37a49d2793c 40 int c=0; //counter for measuring time
alexcrepory 6:a37a49d2793c 41 bool back=0; //180 degrees turn flag
alexcrepory 6:a37a49d2793c 42 int bumps=0; //bumps counter
alexcrepory 6:a37a49d2793c 43 int cw=-1; //turn direction flag
randrews33 4:bf8f4e7c9905 44
alexcrepory 6:a37a49d2793c 45 //Timer ISR, called with 100Hz frequency
alexcrepory 6:a37a49d2793c 46 void timer() {
alexcrepory 6:a37a49d2793c 47 //sensor input
randrews33 4:bf8f4e7c9905 48 dof.readGyro();
alexcrepory 6:a37a49d2793c 49 w=(pi/15)*(dof.calcGyro(dof.gz)-dof.gbias[2]); //measure angular velocity
alexcrepory 6:a37a49d2793c 50 y=y+T*(w-0.0000001); //integrate to obtain orientation
alexcrepory 6:a37a49d2793c 51
alexcrepory 6:a37a49d2793c 52 //input calculation
alexcrepory 6:a37a49d2793c 53 u_star=-K*(y-r);
alexcrepory 6:a37a49d2793c 54
alexcrepory 6:a37a49d2793c 55 //limiting motor speed
alexcrepory 6:a37a49d2793c 56 if (u_star<-umax){
alexcrepory 6:a37a49d2793c 57 u=-umax;
alexcrepory 6:a37a49d2793c 58 } else if (u_star>umax){
alexcrepory 6:a37a49d2793c 59 u=umax;
alexcrepory 6:a37a49d2793c 60 } else{
alexcrepory 6:a37a49d2793c 61 u=u_star;
alexcrepory 6:a37a49d2793c 62 }
alexcrepory 6:a37a49d2793c 63
alexcrepory 6:a37a49d2793c 64 //actuator output
alexcrepory 6:a37a49d2793c 65 vr=(umax+u)/2.0;
alexcrepory 6:a37a49d2793c 66 vl=(umax-u)/2.0;
alexcrepory 6:a37a49d2793c 67 left.speed(vr);
alexcrepory 6:a37a49d2793c 68 right.speed(vl);
alexcrepory 6:a37a49d2793c 69
alexcrepory 6:a37a49d2793c 70 //one second after it bumps, reference changes to -pi or zero
alexcrepory 6:a37a49d2793c 71 if (c==100 && back){
alexcrepory 6:a37a49d2793c 72 r=r+cw*pi/2;
alexcrepory 6:a37a49d2793c 73 c=0;
alexcrepory 6:a37a49d2793c 74 back=0;
alexcrepory 6:a37a49d2793c 75 cw=-cw; //changes direction of 180 turn when bumps
alexcrepory 6:a37a49d2793c 76 pb=0;
alexcrepory 6:a37a49d2793c 77 }
alexcrepory 6:a37a49d2793c 78
alexcrepory 6:a37a49d2793c 79 c++;
randrews33 0:1b975a6ae539 80 }
randrews33 0:1b975a6ae539 81
alexcrepory 6:a37a49d2793c 82 void bump() {
alexcrepory 6:a37a49d2793c 83 bumps++; //counts bumps
alexcrepory 6:a37a49d2793c 84 tmr.detach(); //turn off timer
alexcrepory 6:a37a49d2793c 85 ld4=1;
randrews33 4:bf8f4e7c9905 86
alexcrepory 6:a37a49d2793c 87 //avoid abrupt speed changes
alexcrepory 6:a37a49d2793c 88 left.speed(0);
alexcrepory 6:a37a49d2793c 89 right.speed(0);
alexcrepory 6:a37a49d2793c 90 wait(0.5);
alexcrepory 6:a37a49d2793c 91
alexcrepory 6:a37a49d2793c 92 //goes backwards for 0.5s
alexcrepory 6:a37a49d2793c 93 left.speed(-0.8);
alexcrepory 6:a37a49d2793c 94 right.speed(-0.8);
alexcrepory 6:a37a49d2793c 95 wait(0.5);
randrews33 0:1b975a6ae539 96
alexcrepory 6:a37a49d2793c 97 ld4=0;
alexcrepory 6:a37a49d2793c 98 pb=1;
alexcrepory 6:a37a49d2793c 99 c=0;
alexcrepory 6:a37a49d2793c 100
alexcrepory 6:a37a49d2793c 101 //change reference to plus or minus pi/2
alexcrepory 6:a37a49d2793c 102 r=r+cw*pi/2;
randrews33 0:1b975a6ae539 103
alexcrepory 6:a37a49d2793c 104 //implementation of finding initial position
alexcrepory 6:a37a49d2793c 105 if (bumps>2){
alexcrepory 6:a37a49d2793c 106 ld3=1;
alexcrepory 6:a37a49d2793c 107 back=1;
alexcrepory 6:a37a49d2793c 108 }
alexcrepory 6:a37a49d2793c 109
alexcrepory 6:a37a49d2793c 110 LPC_TIM3->TC = 0;
alexcrepory 6:a37a49d2793c 111 tmr.attach(&timer, T); //turn timer back on
randrews33 0:1b975a6ae539 112 }
randrews33 0:1b975a6ae539 113
randrews33 0:1b975a6ae539 114 int main()
randrews33 0:1b975a6ae539 115 {
alexcrepory 6:a37a49d2793c 116 uint16_t status = dof.begin(); //start and setup gyroscope
alexcrepory 6:a37a49d2793c 117 dof.calLSM9DS0(dof.gbias, dof.abias); //Calculate bias
alexcrepory 6:a37a49d2793c 118
alexcrepory 6:a37a49d2793c 119 wait(3);
alexcrepory 6:a37a49d2793c 120 //starts going forward with 0.5 speed in both motors
alexcrepory 6:a37a49d2793c 121 left.speed(vl);
alexcrepory 6:a37a49d2793c 122 right.speed(vr);
alexcrepory 6:a37a49d2793c 123
alexcrepory 6:a37a49d2793c 124 button.rise(&bump); //atach bump external interrupt
alexcrepory 6:a37a49d2793c 125
alexcrepory 6:a37a49d2793c 126 //start timer with 0.01 period
alexcrepory 6:a37a49d2793c 127 timer_on=1;
alexcrepory 6:a37a49d2793c 128 tmr.attach(&timer, T);
randrews33 4:bf8f4e7c9905 129
alexcrepory 6:a37a49d2793c 130 while (true){} //infinite loop
alexcrepory 6:a37a49d2793c 131 }