ryo seki / Mbed 2 deprecated ver1_2_2_1

test

Dependencies:   HMC6352 PID mbed

Fork of ver1_2_2 by ryo seki

main.cpp@0:74bf4953c0d1, 2013-03-09 (annotated)

Committer:
akudohune
Date:
Sat Mar 09 10:11:06 2013 +0000
Revision:
0:74bf4953c0d1
Child:
1:89408fff7cc9
ver1_2_2;

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