ryo seki / Mbed 2 deprecated ver1_2_2_1

test

Dependencies:   HMC6352 PID mbed

Fork of ver1_2_2 by ryo seki

main.cpp@1:89408fff7cc9, 2013-03-10 (annotated)

Committer:
akudohune
Date:
Sun Mar 10 07:31:31 2013 +0000
Revision:
1:89408fff7cc9
Parent:
0:74bf4953c0d1 
new_cup;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akudohune 0:74bf4953c0d1 1 #include <math.h>
akudohune 0:74bf4953c0d1 2 #include <sstream>
akudohune 0:74bf4953c0d1 3 #include "mbed.h"
akudohune 0:74bf4953c0d1 4 #include "HMC6352.h"
akudohune 0:74bf4953c0d1 5 #include "PID.h"
akudohune 0:74bf4953c0d1 6 #include "main.h"
akudohune 0:74bf4953c0d1 7
akudohune 0:74bf4953c0d1 8
akudohune 1:89408fff7cc9 9
akudohune 1:89408fff7cc9 10 void PidUpdata()
akudohune 1:89408fff7cc9 11 {
akudohune 1:89408fff7cc9 12 inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);
akudohune 1:89408fff7cc9 13
akudohune 1:89408fff7cc9 14 //pc.printf("%f\n",timer1.read());
akudohune 1:89408fff7cc9 15 pid.setProcessValue(inputPID);
akudohune 1:89408fff7cc9 16 //timer1.reset();
akudohune 1:89408fff7cc9 17
akudohune 1:89408fff7cc9 18 compassPID = -(pid.compute());
akudohune 1:89408fff7cc9 19
akudohune 1:89408fff7cc9 20 //pc.printf("%f\n",compassPID);
akudohune 1:89408fff7cc9 21
akudohune 1:89408fff7cc9 22 }
akudohune 1:89408fff7cc9 23
akudohune 1:89408fff7cc9 24 void move(int vxx, int vyy, int vss)
akudohune 1:89408fff7cc9 25 {
akudohune 1:89408fff7cc9 26 double motVal[MOT_NUM] = {0};
akudohune 1:89408fff7cc9 27
akudohune 1:89408fff7cc9 28 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
akudohune 1:89408fff7cc9 29 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2);
akudohune 1:89408fff7cc9 30 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
akudohune 1:89408fff7cc9 31 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4);
akudohune 1:89408fff7cc9 32
akudohune 1:89408fff7cc9 33 for(uint8_t i = 0 ; i < MOT_NUM ; i++){
akudohune 1:89408fff7cc9 34 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
akudohune 1:89408fff7cc9 35 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
akudohune 1:89408fff7cc9 36 speed[i] = motVal[i];
akudohune 1:89408fff7cc9 37 }
akudohune 1:89408fff7cc9 38 /*
akudohune 1:89408fff7cc9 39 pc.printf("speed1 = %d\n",speed[0]);
akudohune 1:89408fff7cc9 40 pc.printf("speed2 = %d\n",speed[1]);
akudohune 1:89408fff7cc9 41 pc.printf("speed3 = %d\n",speed[2]);
akudohune 1:89408fff7cc9 42 pc.printf("speed4 = %d\n\n",speed[3]);
akudohune 1:89408fff7cc9 43 */
akudohune 1:89408fff7cc9 44 ////pc.printf("%s",StringFIN.c_str());
akudohune 1:89408fff7cc9 45 }
akudohune 1:89408fff7cc9 46
akudohune 0:74bf4953c0d1 47 /*********** Serial interrupt ***********/
akudohune 0:74bf4953c0d1 48
akudohune 0:74bf4953c0d1 49 void Tx_interrupt()
akudohune 0:74bf4953c0d1 50 {
akudohune 0:74bf4953c0d1 51 array(speed[0],speed[1],speed[2],speed[3]);
akudohune 0:74bf4953c0d1 52 driver.printf("%s",StringFIN.c_str());
akudohune 0:74bf4953c0d1 53 //pc.printf("%s",StringFIN.c_str());
akudohune 0:74bf4953c0d1 54 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:74bf4953c0d1 55 }
akudohune 0:74bf4953c0d1 56 /*
akudohune 0:74bf4953c0d1 57 void Rx_interrupt()
akudohune 0:74bf4953c0d1 58 {
akudohune 0:74bf4953c0d1 59 if(driver.readable()){
akudohune 0:74bf4953c0d1 60 //pc.printf("%d\n",driver.getc());
akudohune 0:74bf4953c0d1 61 }
akudohune 0:74bf4953c0d1 62 }*/
akudohune 0:74bf4953c0d1 63
akudohune 0:74bf4953c0d1 64
akudohune 0:74bf4953c0d1 65 /*********** Serial interrupt end **********/
akudohune 0:74bf4953c0d1 66
akudohune 0:74bf4953c0d1 67
akudohune 0:74bf4953c0d1 68 void init()
akudohune 0:74bf4953c0d1 69 {
akudohune 0:74bf4953c0d1 70
akudohune 0:74bf4953c0d1 71 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
akudohune 0:74bf4953c0d1 72 StartButton.mode(PullUp);
akudohune 0:74bf4953c0d1 73 CalibEnterButton.mode(PullUp);
akudohune 0:74bf4953c0d1 74 CalibExitButton.mode(PullUp);
akudohune 0:74bf4953c0d1 75 driver.baud(BAUD_RATE);
akudohune 0:74bf4953c0d1 76 wait_ms(MOTDRIVER_WAIT);
akudohune 0:74bf4953c0d1 77 driver.printf("1F0002F0003F0004F000\r\n");
akudohune 0:74bf4953c0d1 78
akudohune 0:74bf4953c0d1 79 led1 = ON;
akudohune 0:74bf4953c0d1 80
akudohune 0:74bf4953c0d1 81 while(StartButton){
akudohune 0:74bf4953c0d1 82 if(!CalibEnterButton){
akudohune 0:74bf4953c0d1 83 led1 = OFF;
akudohune 0:74bf4953c0d1 84 led2 = ON;
akudohune 0:74bf4953c0d1 85 compass.setCalibrationMode(ENTER);
akudohune 0:74bf4953c0d1 86 while(CalibExitButton);
akudohune 0:74bf4953c0d1 87 compass.setCalibrationMode(EXIT);
akudohune 0:74bf4953c0d1 88 led2 = OFF;
akudohune 0:74bf4953c0d1 89 led3 = ON;
akudohune 0:74bf4953c0d1 90 }
akudohune 0:74bf4953c0d1 91 }
akudohune 0:74bf4953c0d1 92
akudohune 0:74bf4953c0d1 93 standard = compass.sample() / 10.0;
akudohune 1:89408fff7cc9 94
akudohune 0:74bf4953c0d1 95 led1 = OFF;
akudohune 0:74bf4953c0d1 96 led3 = OFF;
akudohune 0:74bf4953c0d1 97
akudohune 0:74bf4953c0d1 98 pid.setInputLimits(0.0, 360.0);
akudohune 0:74bf4953c0d1 99 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
akudohune 0:74bf4953c0d1 100 pid.setBias(0.0);
akudohune 0:74bf4953c0d1 101 pid.setMode(AUTO_MODE);
akudohune 0:74bf4953c0d1 102 pid.setSetPoint(180.0);
akudohune 0:74bf4953c0d1 103
akudohune 0:74bf4953c0d1 104 pidUpdata.attach(&PidUpdata, 0.06);
akudohune 0:74bf4953c0d1 105 wait(1);
akudohune 0:74bf4953c0d1 106 IR.attach(&IR_Position,0.04);
akudohune 0:74bf4953c0d1 107 ultrasonic.attach(&Ultrasonic, 0.05);
akudohune 0:74bf4953c0d1 108 driver.attach(&Tx_interrupt, Serial::TxIrq);
akudohune 0:74bf4953c0d1 109 //driver.attach(&Rx_interrupt, Serial::RxIrq);
akudohune 0:74bf4953c0d1 110
akudohune 0:74bf4953c0d1 111 timer1.start();
akudohune 0:74bf4953c0d1 112 timer2.start();
akudohune 0:74bf4953c0d1 113 }
akudohune 0:74bf4953c0d1 114
akudohune 0:74bf4953c0d1 115 int main()
akudohune 0:74bf4953c0d1 116 {
akudohune 0:74bf4953c0d1 117 int vx=0,vy=0,vs=0;
akudohune 0:74bf4953c0d1 118 int state = HOME_WAIT;
akudohune 0:74bf4953c0d1 119
akudohune 0:74bf4953c0d1 120 init();
akudohune 0:74bf4953c0d1 121
akudohune 0:74bf4953c0d1 122 while(1) {
akudohune 0:74bf4953c0d1 123
akudohune 0:74bf4953c0d1 124 vs = compassPID;
akudohune 0:74bf4953c0d1 125 //vx = 15;
akudohune 0:74bf4953c0d1 126 //vy = 10;
akudohune 1:89408fff7cc9 127
akudohune 0:74bf4953c0d1 128 /*
akudohune 0:74bf4953c0d1 129 if(IR_found){
akudohune 0:74bf4953c0d1 130 if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
akudohune 1:89408fff7cc9 131 vx = (int)(70*ball_sankaku[direction][0]);
akudohune 1:89408fff7cc9 132 vy = (int)(70*ball_sankaku[direction][1]);
akudohune 0:74bf4953c0d1 133 }else{
akudohune 1:89408fff7cc9 134 vx = (int)(70*ball_sankaku[direction][0]);
akudohune 1:89408fff7cc9 135 vy = (int)(70*ball_sankaku[direction][1]);
akudohune 0:74bf4953c0d1 136 }
akudohune 0:74bf4953c0d1 137 }else{
akudohune 0:74bf4953c0d1 138 vx = 0;
akudohune 0:74bf4953c0d1 139 vy = 0;
akudohune 0:74bf4953c0d1 140 }
akudohune 0:74bf4953c0d1 141 */
akudohune 1:89408fff7cc9 142
akudohune 0:74bf4953c0d1 143 /*
akudohune 0:74bf4953c0d1 144 if(IR_found)state = DIFFENCE;
akudohune 0:74bf4953c0d1 145 else state = HOME_WAIT;
akudohune 1:89408fff7cc9 146 */
akudohune 1:89408fff7cc9 147 /*
akudohune 0:74bf4953c0d1 148 if(state == HOME_WAIT){
akudohune 0:74bf4953c0d1 149 if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
akudohune 0:74bf4953c0d1 150 if(ultrasonicVal[0] > 3200){
akudohune 0:74bf4953c0d1 151 vx = 15;
akudohune 0:74bf4953c0d1 152 vy = 0;
akudohune 0:74bf4953c0d1 153 }else if(ultrasonicVal[2] > 3200){
akudohune 0:74bf4953c0d1 154 vx = -15;
akudohune 0:74bf4953c0d1 155 vy = 0;
akudohune 0:74bf4953c0d1 156 }else{
akudohune 1:89408fff7cc9 157 if(ultrasonicVal[1] > 800){
akudohune 0:74bf4953c0d1 158 vx = 0;
akudohune 0:74bf4953c0d1 159 vy = -15;
akudohune 0:74bf4953c0d1 160 }else{
akudohune 0:74bf4953c0d1 161 vx = 0;
akudohune 0:74bf4953c0d1 162 vy = 0;
akudohune 0:74bf4953c0d1 163 }
akudohune 0:74bf4953c0d1 164 }
akudohune 0:74bf4953c0d1 165 }else{
akudohune 0:74bf4953c0d1 166 vx = 0;
akudohune 0:74bf4953c0d1 167 vy = 0;
akudohune 0:74bf4953c0d1 168 }
akudohune 0:74bf4953c0d1 169 }else if(state == DIFFENCE){
akudohune 1:89408fff7cc9 170 if(ultrasonicVal[1] > 800){
akudohune 1:89408fff7cc9 171 vx = 0;
akudohune 1:89408fff7cc9 172 vy = -15;
akudohune 1:89408fff7cc9 173 }else{
akudohune 1:89408fff7cc9 174 if(distance <= 30){
akudohune 1:89408fff7cc9 175
akudohune 1:89408fff7cc9 176 if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
akudohune 1:89408fff7cc9 177 vx = 15;
akudohune 0:74bf4953c0d1 178
akudohune 1:89408fff7cc9 179 }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
akudohune 1:89408fff7cc9 180 vx = -15;
akudohune 0:74bf4953c0d1 181
akudohune 1:89408fff7cc9 182 }else{
akudohune 1:89408fff7cc9 183 vx = 0;
akudohune 1:89408fff7cc9 184 }
akudohune 1:89408fff7cc9 185 }else{
akudohune 1:89408fff7cc9 186
akudohune 1:89408fff7cc9 187 }
akudohune 1:89408fff7cc9 188 }
akudohune 1:89408fff7cc9 189 }
akudohune 1:89408fff7cc9 190 */
akudohune 1:89408fff7cc9 191 /*
akudohune 1:89408fff7cc9 192 if(state == HOME_WAIT){
akudohune 1:89408fff7cc9 193
akudohune 1:89408fff7cc9 194 }*/
akudohune 1:89408fff7cc9 195
akudohune 1:89408fff7cc9 196 if((ultrasonicVal[0] + ultrasonicVal[2]) < 1050.0){
akudohune 1:89408fff7cc9 197 if(ultrasonicVal[0] > 300.0){
akudohune 1:89408fff7cc9 198 vx = 15;
akudohune 1:89408fff7cc9 199 led3 = ON;
akudohune 1:89408fff7cc9 200 led4 = OFF;
akudohune 1:89408fff7cc9 201 }else if(ultrasonicVal[2] > 300.0){
akudohune 1:89408fff7cc9 202 vx = -15;
akudohune 1:89408fff7cc9 203 led3 = ON;
akudohune 1:89408fff7cc9 204 led4 = OFF;
akudohune 1:89408fff7cc9 205 }else{
akudohune 1:89408fff7cc9 206 led3 = OFF;
akudohune 1:89408fff7cc9 207 led4 = ON;
akudohune 1:89408fff7cc9 208 if(ultrasonicVal[1] > 200.0){
akudohune 1:89408fff7cc9 209 vy = -15;
akudohune 1:89408fff7cc9 210 }else if(ultrasonicVal[1] < 100.0){
akudohune 1:89408fff7cc9 211 vy = 15;
akudohune 1:89408fff7cc9 212 }else{
akudohune 1:89408fff7cc9 213 vy = 0;
akudohune 1:89408fff7cc9 214 }
akudohune 1:89408fff7cc9 215 }
akudohune 1:89408fff7cc9 216 led2 = ON;
akudohune 0:74bf4953c0d1 217 }else{
akudohune 0:74bf4953c0d1 218 vx = 0;
akudohune 1:89408fff7cc9 219 vy = 0;
akudohune 1:89408fff7cc9 220 led2 = OFF;
akudohune 0:74bf4953c0d1 221 }
akudohune 1:89408fff7cc9 222
akudohune 1:89408fff7cc9 223 //pc.printf("%f,%f,%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2]);
akudohune 1:89408fff7cc9 224
akudohune 0:74bf4953c0d1 225 move(vx,vy,vs);
akudohune 0:74bf4953c0d1 226 }
akudohune 0:74bf4953c0d1 227 }