Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
main.cpp@1:1da89c13dfa1, 2014-10-11 (annotated)
- Committer:
- Throwbot
- Date:
- Sat Oct 11 03:53:27 2014 +0000
- Revision:
- 1:1da89c13dfa1
- Parent:
- 0:43331220df0d
- Child:
- 2:0f80c8a1c236
Ultrasonic seperate class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Throwbot | 1:1da89c13dfa1 | 1 | // FORMAT_CODE_START |
Throwbot | 0:43331220df0d | 2 | #include "QEI.h" |
Throwbot | 0:43331220df0d | 3 | #include "mbed.h" |
Throwbot | 0:43331220df0d | 4 | #include "robot.h" |
Throwbot | 0:43331220df0d | 5 | #include "rtos.h" |
Throwbot | 1:1da89c13dfa1 | 6 | #include "MPU6050.h" |
Throwbot | 1:1da89c13dfa1 | 7 | #include "I2Cdev.h" |
Throwbot | 1:1da89c13dfa1 | 8 | #include "tone.h" |
Throwbot | 1:1da89c13dfa1 | 9 | #include "ultrasonic.h" |
Throwbot | 0:43331220df0d | 10 | char c; |
Throwbot | 1:1da89c13dfa1 | 11 | char buffer[10]; |
Throwbot | 1:1da89c13dfa1 | 12 | int bit_size=20; |
Throwbot | 1:1da89c13dfa1 | 13 | int thetha1=300; |
Throwbot | 1:1da89c13dfa1 | 14 | int thetha=0; |
Throwbot | 1:1da89c13dfa1 | 15 | int stall=0; |
Throwbot | 1:1da89c13dfa1 | 16 | int bump=1; |
Throwbot | 1:1da89c13dfa1 | 17 | int UL_rR = 0; |
Throwbot | 1:1da89c13dfa1 | 18 | int UL_R = 0; |
Throwbot | 1:1da89c13dfa1 | 19 | int UL_F = 0; |
Throwbot | 1:1da89c13dfa1 | 20 | int UL_L = 0; |
Throwbot | 1:1da89c13dfa1 | 21 | int UL_rL = 0; |
Throwbot | 1:1da89c13dfa1 | 22 | int UL_B = 0; |
Throwbot | 1:1da89c13dfa1 | 23 | int dx1=100; |
Throwbot | 1:1da89c13dfa1 | 24 | int dy1=200; |
Throwbot | 1:1da89c13dfa1 | 25 | |
Throwbot | 1:1da89c13dfa1 | 26 | int linear_velocity_value ; |
Throwbot | 1:1da89c13dfa1 | 27 | int linear_velocity_direction; |
Throwbot | 1:1da89c13dfa1 | 28 | int rotational_velocity_value ; |
Throwbot | 1:1da89c13dfa1 | 29 | int rotational_velocity_direction; |
Throwbot | 1:1da89c13dfa1 | 30 | int Lspeed=1; |
Throwbot | 1:1da89c13dfa1 | 31 | int Rspeed=1; |
Throwbot | 1:1da89c13dfa1 | 32 | |
Throwbot | 1:1da89c13dfa1 | 33 | int main() |
Throwbot | 1:1da89c13dfa1 | 34 | { |
Throwbot | 1:1da89c13dfa1 | 35 | initRobot(); |
Throwbot | 1:1da89c13dfa1 | 36 | //motor_control((100),(-100)); |
Throwbot | 1:1da89c13dfa1 | 37 | while(buffer[3] != 'O') { |
Throwbot | 1:1da89c13dfa1 | 38 | while(buffer[3] != 'E') { |
Throwbot | 1:1da89c13dfa1 | 39 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 40 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 41 | if(buffer[3] == 'E') { |
Throwbot | 1:1da89c13dfa1 | 42 | bt.printf(">>1B"); |
Throwbot | 1:1da89c13dfa1 | 43 | } else if(buffer[3] == '?') { |
Throwbot | 1:1da89c13dfa1 | 44 | bt.printf("K"); |
Throwbot | 1:1da89c13dfa1 | 45 | } |
Throwbot | 1:1da89c13dfa1 | 46 | } |
Throwbot | 1:1da89c13dfa1 | 47 | } |
Throwbot | 1:1da89c13dfa1 | 48 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 49 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 50 | } |
Throwbot | 1:1da89c13dfa1 | 51 | } |
Throwbot | 1:1da89c13dfa1 | 52 | |
Throwbot | 1:1da89c13dfa1 | 53 | //imperial_march(); |
Throwbot | 1:1da89c13dfa1 | 54 | Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 1:1da89c13dfa1 | 55 | Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 1:1da89c13dfa1 | 56 | buzzer=0; |
Throwbot | 1:1da89c13dfa1 | 57 | //motor_control(lMotor*m_speed,rMotor*m_speed*0.8); |
Throwbot | 1:1da89c13dfa1 | 58 | //motor_control(lMotor*m_speed,0); |
Throwbot | 1:1da89c13dfa1 | 59 | while(1) { |
Throwbot | 1:1da89c13dfa1 | 60 | //bt.lock(); |
Throwbot | 1:1da89c13dfa1 | 61 | //bt.printf("LDR1_%lf;LDR2_%lf;Pulses is: %i Revolutions is: %i Pulses is: %i ultrasonic: , \r\n",ldrread1(),ldrread2(), left.getPulses(), left.getRevolutions(),right.getPulses());//, uL.read()*5*102.54*2.54); |
Throwbot | 0:43331220df0d | 62 | //bt.printf("s;LDR1_%lf;LDR2_%lf;Pulses is: %i ; %i:s\n\r",ldrread1(),ldrread2(),wheel.getPulses(), wheel.getRevolutions()); |
Throwbot | 1:1da89c13dfa1 | 63 | // motor_control(lMotor*m_speed,rMotor*m_speed); |
Throwbot | 1:1da89c13dfa1 | 64 | //bt.printf("a/g:\t %d:\t %d:\t %d:\t %d:\t %d:\t %d:\t\r\n",ax ,ay,az,gx,gy,gz); |
Throwbot | 1:1da89c13dfa1 | 65 | //bt.unlock(); |
Throwbot | 1:1da89c13dfa1 | 66 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 67 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 68 | if(buffer[3] == 'S') { |
Throwbot | 1:1da89c13dfa1 | 69 | SRX=1; |
Throwbot | 1:1da89c13dfa1 | 70 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 71 | UL_rR=sensorvalue(urR); |
Throwbot | 1:1da89c13dfa1 | 72 | SRX=0; |
Throwbot | 1:1da89c13dfa1 | 73 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 74 | |
Throwbot | 1:1da89c13dfa1 | 75 | SRX=1; |
Throwbot | 1:1da89c13dfa1 | 76 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 77 | UL_R=sensorvalue(uR); |
Throwbot | 1:1da89c13dfa1 | 78 | SRX = 0; |
Throwbot | 1:1da89c13dfa1 | 79 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 80 | |
Throwbot | 1:1da89c13dfa1 | 81 | SRX=1; |
Throwbot | 1:1da89c13dfa1 | 82 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 83 | UL_F=sensorvalue(uF); |
Throwbot | 1:1da89c13dfa1 | 84 | SRX=0; |
Throwbot | 1:1da89c13dfa1 | 85 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 86 | |
Throwbot | 1:1da89c13dfa1 | 87 | SRX=1; |
Throwbot | 1:1da89c13dfa1 | 88 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 89 | UL_L=sensorvalue(uL); |
Throwbot | 1:1da89c13dfa1 | 90 | SRX=0; |
Throwbot | 1:1da89c13dfa1 | 91 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 92 | |
Throwbot | 1:1da89c13dfa1 | 93 | SRX=1; |
Throwbot | 1:1da89c13dfa1 | 94 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 95 | UL_rL=sensorvalue(urL); |
Throwbot | 1:1da89c13dfa1 | 96 | SRX=0; |
Throwbot | 1:1da89c13dfa1 | 97 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 98 | |
Throwbot | 1:1da89c13dfa1 | 99 | SRX=1; |
Throwbot | 1:1da89c13dfa1 | 100 | wait_ms(2); |
Throwbot | 1:1da89c13dfa1 | 101 | UL_B=sensorvalue(uB); |
Throwbot | 1:1da89c13dfa1 | 102 | SRX=0; |
Throwbot | 1:1da89c13dfa1 | 103 | |
Throwbot | 1:1da89c13dfa1 | 104 | bt.lock(); |
Throwbot | 1:1da89c13dfa1 | 105 | stdio_mutex.lock(); |
Throwbot | 1:1da89c13dfa1 | 106 | heading=heading*11.375; |
Throwbot | 1:1da89c13dfa1 | 107 | bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", |
Throwbot | 1:1da89c13dfa1 | 108 | dx/256,dx%256,dy/256,dy%256,\ |
Throwbot | 1:1da89c13dfa1 | 109 | (heading)/256,(heading)%256,\ |
Throwbot | 1:1da89c13dfa1 | 110 | stall, bump,\ |
Throwbot | 1:1da89c13dfa1 | 111 | UL_rR/256,UL_rR%256,\ |
Throwbot | 1:1da89c13dfa1 | 112 | UL_R/256,UL_R%256, \ |
Throwbot | 1:1da89c13dfa1 | 113 | UL_F/256,UL_F%256,\ |
Throwbot | 1:1da89c13dfa1 | 114 | UL_L/256,UL_L%256,\ |
Throwbot | 1:1da89c13dfa1 | 115 | UL_rL/256,UL_rL%256,\ |
Throwbot | 1:1da89c13dfa1 | 116 | UL_B/256,UL_B%256); |
Throwbot | 1:1da89c13dfa1 | 117 | dx=0; |
Throwbot | 1:1da89c13dfa1 | 118 | dy=0; |
Throwbot | 1:1da89c13dfa1 | 119 | stdio_mutex.unlock(); |
Throwbot | 1:1da89c13dfa1 | 120 | bt.unlock(); |
Throwbot | 1:1da89c13dfa1 | 121 | } else if (buffer[3] == 'R') { |
Throwbot | 1:1da89c13dfa1 | 122 | Thread::wait(20); |
Throwbot | 1:1da89c13dfa1 | 123 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 124 | bt.gets(buffer, 10); |
Throwbot | 1:1da89c13dfa1 | 125 | linear_velocity_value = buffer[4]<<8|buffer[3]; |
Throwbot | 1:1da89c13dfa1 | 126 | linear_velocity_direction= buffer[5]; |
Throwbot | 1:1da89c13dfa1 | 127 | rotational_velocity_value = buffer[7]<<8|buffer[6]; |
Throwbot | 1:1da89c13dfa1 | 128 | rotational_velocity_direction= buffer[8]; |
Throwbot | 1:1da89c13dfa1 | 129 | if( linear_velocity_direction==0x01) { |
Throwbot | 1:1da89c13dfa1 | 130 | Lspeed=lMotor; |
Throwbot | 1:1da89c13dfa1 | 131 | Rspeed=rMotor; |
Throwbot | 1:1da89c13dfa1 | 132 | } else if( linear_velocity_direction==0x10) { |
Throwbot | 1:1da89c13dfa1 | 133 | Lspeed=-lMotor; |
Throwbot | 1:1da89c13dfa1 | 134 | Rspeed=-rMotor; |
Throwbot | 1:1da89c13dfa1 | 135 | } |
Throwbot | 1:1da89c13dfa1 | 136 | if( rotational_velocity_direction==0x01 && linear_velocity_value !=0) { |
Throwbot | 1:1da89c13dfa1 | 137 | |
Throwbot | 1:1da89c13dfa1 | 138 | |
Throwbot | 1:1da89c13dfa1 | 139 | motor_control((Lspeed*linear_velocity_value/35)-10,(Rspeed*linear_velocity_value/35) + 10); |
Throwbot | 1:1da89c13dfa1 | 140 | |
Throwbot | 1:1da89c13dfa1 | 141 | } else if( rotational_velocity_direction==0x10 && linear_velocity_value !=0) { |
Throwbot | 1:1da89c13dfa1 | 142 | motor_control((Lspeed*linear_velocity_value/35)+10,(Rspeed*linear_velocity_value/30) -10); |
Throwbot | 1:1da89c13dfa1 | 143 | } |
Throwbot | 1:1da89c13dfa1 | 144 | else |
Throwbot | 1:1da89c13dfa1 | 145 | motor_control((Lspeed*linear_velocity_value/30),(Rspeed*linear_velocity_value/30)); |
Throwbot | 1:1da89c13dfa1 | 146 | |
Throwbot | 1:1da89c13dfa1 | 147 | } |
Throwbot | 1:1da89c13dfa1 | 148 | } |
Throwbot | 1:1da89c13dfa1 | 149 | //memset(buffer, 0, sizeof buffer); |
Throwbot | 0:43331220df0d | 150 | } |
Throwbot | 1:1da89c13dfa1 | 151 | |
Throwbot | 1:1da89c13dfa1 | 152 | if(ldrread1()>0.6) { |
Throwbot | 1:1da89c13dfa1 | 153 | } |
Throwbot | 1:1da89c13dfa1 | 154 | if(ldrread2()>0.6) { |
Throwbot | 1:1da89c13dfa1 | 155 | Led_on(); |
Throwbot | 1:1da89c13dfa1 | 156 | } else { |
Throwbot | 1:1da89c13dfa1 | 157 | Led_off(); |
Throwbot | 1:1da89c13dfa1 | 158 | } |
Throwbot | 1:1da89c13dfa1 | 159 | Thread::wait(20); |
Throwbot | 0:43331220df0d | 160 | } |
Throwbot | 0:43331220df0d | 161 | } |